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;
}
}
|
