summaryrefslogtreecommitdiff
path: root/segway.c
blob: e94ccabc7df1d134d0bda98ad88ea66cb60bae45 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
/*
 * Notes variées sur le code original:
 *  * Gérard utilise trois mesures pour déterminer la vitesse des moteurs:
 *    - l'angle
 *    - la vitesse angulaire
 *    - la vitesse linéaire
 *  * Gérard adapte le gain en fonction du niveau des piles (pas bête).
 *  * Gérard prend en compte la variation sur le long terme de l'angle,
 *    probablement pour contrebalancer l'imprécision du capteur (ceci pourrait
 *    expliquer nos échecs passés).
 *  * Gérard code comme un goret.
 *
 */

const tSensors Gyro = (tSensors) S1; /* Our gyro sensor */
#define GyroOffset  618.0

#define t_scale     1000.0
#define half_dt     2

#define k_ang_vel   10.0
#define k_ang       8.0 

#define k_bias      0.001

task main ()
{
    float theta = 0, theta_old = 0;
    float theta_bias = 0;
    float t;
    long r1 = 0, r2 = 0;
    float pwr = 0;

    /* Initialize motors */
    nMotorEncoder[motorC] = 0;
    nMotorEncoder[motorA] = 0;
    /* This causes the motors to stop when they are set to zero */
	bFloatDuringInactiveMotorPWM = false;

    t = nSysTime;
 	r1 = 0;

    batt = nAvgBatteryLevel;
    k_pwr = 0.7 + (0.7 - 1.1)/(8816 - 8196) * (batt - 8816);

    while (true)
    {
        wait1Msec (half_dt);
        r2 = SensorRaw (GyroSensor) - GyroOffset; /* theta' (t + dt / 2) */
        wait1Msec (half_dt);
        /* theta (t + dt) = theta (t) + (theta' (t)
         *                             + 2 * theta' (t + dt / 2)) / 3 * dt */
        theta = theta_old + (r1 + 2.0 * r2) * (nSysTime - t) / (3.0 * t_scale);
        theta_old = theta;
 		r1 = SensorRaw (GyroSensor) - GyroBias; /* theta' (t) */
 		t = nSysTime;

        theta_bias = theta_bias * (1.0 - k_bias) + theta * k_bias;

 		pwr = k_ang_vel * r1 + k_ang * (theta - theta_bias);
		motor[motorA] = pwr;
		motor[motorC] = pwr;
 	}
}