summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorGuillaume Seguin <guillaume@segu.in>2008-02-13 13:03:33 +0100
committerGuillaume Seguin <guillaume@segu.in>2008-02-13 13:03:33 +0100
commit38a76f593fd56e7b0a0dc5971ec3b2a209b43ece (patch)
tree8c6db585bb0d10c483a983b3aa693cf54b49002a
parentc5bdc3355dc1c1738e7cc0c242eb92e8b8f0684e (diff)
downloadsegway-38a76f593fd56e7b0a0dc5971ec3b2a209b43ece.tar.gz
segway-38a76f593fd56e7b0a0dc5971ec3b2a209b43ece.tar.bz2
* Separate angle computation from actual control code
-rw-r--r--segway.c172
1 files changed, 89 insertions, 83 deletions
diff --git a/segway.c b/segway.c
index 2dec738..e654216 100644
--- a/segway.c
+++ b/segway.c
@@ -17,54 +17,80 @@ 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 NoiseLimit 1
-#define t_scale 1000.0
-#define half_dt 3
-#define dt 3
-#define k_gyro 1.05
+#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_ang_vel 5.0
+#define k_ang 8.0
-#define k_sync 2 /* Keep the two wheels in sync */
+#define k_sync 2 /* Keep the two wheels in sync */
-#define k_bias 0.001
+#define k_bias 0.001
-#define min_dist 30
+#define min_dist 30
-#define timeout 300
+#define timeout 300
+
+typedef enum
+{
+ StateWarmup,
+ StateRunning,
+} GyroState;
+GyroState state = StateWarmup;
+
+float theta = 0;
inline float min (float v1, float v2)
{
- if (v1 > v2)
- return v2;
- else
- return v1;
+ if (v1 > v2)
+ return v2;
+ else
+ return v1;
}
inline float max (float v1, float v2)
{
- if (v1 > v2)
- return v1;
- else
- return v2;
+ if (v1 > v2)
+ return v1;
+ else
+ return v2;
+}
+
+task AngleComputer ()
+{
+ float r1 = 0, r2 = 0;
+
+ /* Increase execution priority */
+ nSchedulePriority = kHighPriority - 1;
+
+ wait1Msec (2000); /* Startup delay */
+
+ state = StateRunning;
+
+ while (true)
+ {
+ wait1Msec (dt);
+ r1 = SensorRaw (GyroSensor1) - SensorRaw (GyroSensor2);
+ r2 = ((float) r1 - GyroOffset1 + GyroOffset2) / 2.0;
+ if (r2 <= NoiseLimit && r2 >= - NoiseLimit)
+ r2 = 0;
+ theta += + r2 * -0.00307;
+ }
+ return;
}
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;
+ int full_steps = 0;
- /* Prepare sensors */
- //SetSensorType (SonarSensor, sensorSONAR);
- nSchedulePriority = kHighPriority - 1; // Boost execution priority so that the task runs deterministically
+ StartTask (AngleComputer);
/* Initialize motors */
nMotorEncoder[motorC] = 0;
@@ -72,66 +98,46 @@ task main ()
/* This causes the motors to stop when they are set to zero */
bFloatDuringInactiveMotorPWM = false;
- wait1Msec (2000); /* Startup delay */
+ while (state != StateRunning)
+ wait1Msec (5);
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;
- }
- }
+ 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]);
+ motor[motorC] = pwr;
+
+ /* Security check to stop program if the robot has fallen */
+ if (pwr > 100 || pwr < - 100)
+ full_steps += 1;
+ else
+ full_steps = 0;
+ if (full_steps > timeout)
+ {
+ motor[motorA] = 0;
+ motor[motorC] = 0;
+ break;
+ }
+ }
+ StopAllTasks ();
+ return;
}