summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorGuillaume Seguin <guillaume@segu.in>2008-02-13 12:10:46 +0100
committerGuillaume Seguin <guillaume@segu.in>2008-02-13 12:10:46 +0100
commitc5bdc3355dc1c1738e7cc0c242eb92e8b8f0684e (patch)
treeae19ab365a44818c5e8c48f43e15785c8b1c2a13
parent89b586543d9487c4c693225821ff5a4d078e061f (diff)
downloadsegway-c5bdc3355dc1c1738e7cc0c242eb92e8b8f0684e.tar.gz
segway-c5bdc3355dc1c1738e7cc0c242eb92e8b8f0684e.tar.bz2
* Major rework of segway.c, it works!
-rw-r--r--segway.c201
1 files changed, 137 insertions, 64 deletions
diff --git a/segway.c b/segway.c
index e94ccab..2dec738 100644
--- a/segway.c
+++ b/segway.c
@@ -1,64 +1,137 @@
-/*
- * 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;
- }
-}
+/*
+ * Notes variees sur le code original:
+ * * Gerard utilise trois mesures pour déterminer la vitesse des moteurs:
+ * - l'angle
+ * - la vitesse angulaire
+ * - la vitesse lineaire
+ * * Gerard adapte le gain en fonction du niveau des piles (pas bête).
+ * * Gerard prend en compte la variation sur le long terme de l'angle,
+ * probablement pour contrebalancer l'imprecision du capteur (ceci pourrait
+ * expliquer nos echecs passes).
+ * * Gerard code comme un goret.
+ *
+ */
+
+const tSensors GyroSensor1 = (tSensors) S1; /* Our first gyro sensor */
+const tSensors GyroSensor2 = (tSensors) S2; /* Our second gyro sensor */
+const tSensors SonarSensor = (tSensors) S3; /* Our sonar sensor */
+#define GyroOffset1 606.
+#define GyroOffset2 615.
+#define NoiseLimit 1
+
+#define t_scale 1000.0
+#define half_dt 3
+#define dt 3
+#define k_gyro 1.05
+
+#define k_ang_vel 5.0
+#define k_ang 8.0
+
+#define k_sync 2 /* Keep the two wheels in sync */
+
+#define k_bias 0.001
+
+#define min_dist 30
+
+#define timeout 300
+
+inline float min (float v1, float v2)
+{
+ if (v1 > v2)
+ return v2;
+ else
+ return v1;
+}
+
+inline float max (float v1, float v2)
+{
+ if (v1 > v2)
+ return v1;
+ else
+ return v2;
+}
+
+task main ()
+{
+ float theta = 0;
+ float theta_bias = 0;
+ float t;
+ float r1 = 0, r2 = 0;
+ float pwr = 0;
+ int full_steps = 0;
+
+ float dist = 0;
+
+ /* Prepare sensors */
+ //SetSensorType (SonarSensor, sensorSONAR);
+ nSchedulePriority = kHighPriority - 1; // Boost execution priority so that the task runs deterministically
+
+ /* Initialize motors */
+ nMotorEncoder[motorC] = 0;
+ nMotorEncoder[motorA] = 0;
+ /* This causes the motors to stop when they are set to zero */
+ bFloatDuringInactiveMotorPWM = false;
+
+ wait1Msec (2000); /* Startup delay */
+
+ while (true)
+ {
+ wait1Msec (dt);
+ r1 = SensorRaw (GyroSensor1) - SensorRaw (GyroSensor2);
+ r2 = ((float) r1 - GyroOffset1 + GyroOffset2) / 2.0;
+ /* theta' (t + dt / 2) */
+ if (r2 <= NoiseLimit && r2 >= - NoiseLimit)
+ r2 = 0;
+ dist = SensorRaw (SonarSensor);
+ // r2 = 0;
+ //wait1Msec (half_dt);
+ /* theta (t + dt) = theta (t) + (theta' (t)
+ * + 2 * theta' (t + dt / 2)) / 3 * dt */
+ // r1 = (float) SensorRaw (GyroSensor) - GyroOffset; /* theta' (t) */
+ /*if (r1 <= 0.2 && r1 >= - 0.2)
+ r1 = 0;*/
+
+ //theta = theta_old - r2 / (dt * t_scale) * k_gyro;
+ theta = theta + r2 * -0.00307;
+ theta_bias = theta_bias * (1.0 - k_bias) + theta * k_bias;
+
+ t = nSysTime;
+
+ pwr = theta - theta_bias;
+
+ if (abs (pwr) < 0.01)
+ {
+ pwr = 0;
+ }
+ else if (abs (pwr) < 2)
+ {
+ if (pwr > 0)
+ pwr = min (20, 8 + k_ang * pwr);
+ else
+ pwr = max (-20, -8 + k_ang * pwr);
+ }
+ else if (pwr < 0)
+ {
+ pwr = - 200;
+ }
+ else
+ {
+ pwr = 200;
+ }
+ motor[motorA] = pwr - k_sync * (nMotorEncoder[motorA] - nMotorEncoder[motorC]);
+ //- 16 + 6.5 * (nMotorEncoder[motorA] - nMotorEncoder[motorC]);
+ motor[motorC] = pwr;
+ //+16;
+
+ if (pwr > 100 || pwr < - 100)
+ full_steps += 1;
+ else
+ full_steps = 0;
+ if (full_steps > timeout)
+ {
+ motor[motorA] = 0;
+ motor[motorC] = 0;
+ break;
+ }
+ }
+}