diff options
| -rw-r--r-- | segway.c | 201 |
1 files changed, 137 insertions, 64 deletions
@@ -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;
+ }
+ }
+}
|