summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorGuillaume Seguin <guillaume@segu.in>2008-02-13 12:10:19 +0100
committerGuillaume Seguin <guillaume@segu.in>2008-02-13 12:10:19 +0100
commit89b586543d9487c4c693225821ff5a4d078e061f (patch)
tree0f0f56c950ac6978ea7217a9aac6360ad47b6bb9
parentf58031717ba0305967c70c6b8527d481375b0df1 (diff)
downloadsegway-89b586543d9487c4c693225821ff5a4d078e061f.tar.gz
segway-89b586543d9487c4c693225821ff5a4d078e061f.tar.bz2
* Import segway.c
-rw-r--r--segway.c64
1 files changed, 64 insertions, 0 deletions
diff --git a/segway.c b/segway.c
new file mode 100644
index 0000000..e94ccab
--- /dev/null
+++ b/segway.c
@@ -0,0 +1,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;
+ }
+}