summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorGuillaume Seguin <guillaume@segu.in>2008-03-09 23:37:35 +0100
committerGuillaume Seguin <guillaume@segu.in>2008-03-09 23:37:35 +0100
commit5eaa13d04814fcd8b66e8c54d9e53d288b4f1f21 (patch)
tree909dbe4fdbb339de037c5f619149338d1ea947ad
parent72db83894a2337880ff02a0cd1254a246268da86 (diff)
downloadsegway-master.tar.gz
segway-master.tar.bz2
* Reimport a dual-gyro base, which is the best source we've had for nowHEADmaster
-rwxr-xr-xbisegway.c199
1 files changed, 199 insertions, 0 deletions
diff --git a/bisegway.c b/bisegway.c
new file mode 100755
index 0000000..accd179
--- /dev/null
+++ b/bisegway.c
@@ -0,0 +1,199 @@
+const tSensors GyroSensor1 = (tSensors) S1; /* Our first gyro sensor */
+const tSensors GyroSensor2 = (tSensors) S3; /* Our second gyro sensor */
+
+#define NoiseLimit 1
+
+#define dt 3
+#define offset_dt 40
+#define post_timeout_dt 1000
+
+#define k_offset 0.001
+#define k_offset2 0.2
+#define k_tdot 0.2
+
+#define k_pos 0.05
+#define target_pos 0
+#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 100
+
+#define log_dt 10
+#define log_samples 2000
+
+typedef enum
+{
+ StateWarmup,
+ StateRunning,
+} GyroState;
+GyroState state = StateWarmup;
+
+float theta = 0;
+float theta_dot = 0;
+
+/* Helper functions */
+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;
+}
+
+/* Detached thread that computes the current angle based on data from Gyros */
+task AngleComputer ()
+{
+ float raw1 = 0, raw2 = 0, delta = 0;
+ float bi_offset;
+
+ /* Increase execution priority */
+ nSchedulePriority = kHighPriority - 1;
+ SensorType[GyroSensor1] = sensorLightInactive;
+ SensorType[GyroSensor2] = sensorLightInactive;
+
+ wait1Msec (1000); /* Startup delay */
+ for (int i = 0; i < 300; i++)
+ {
+ wait1Msec (2 * dt);
+ raw1 = SensorRaw (GyroSensor1);
+ raw2 = SensorRaw (GyroSensor2);
+ delta = raw1 - raw2;
+ bi_offset = (1 - k_offset) * bi_offset + k_offset * delta;
+ }
+ bi_offset = 8;
+
+ state = StateRunning;
+
+ while (true)
+ {
+ wait1Msec (dt);
+ /* Compute the angle with data given by first Gyro */
+ raw1 = SensorRaw (GyroSensor1);
+ raw2 = SensorRaw (GyroSensor2);
+ delta = raw1 - raw2;
+ /* Offset it correctly */
+ theta_dot = (delta - bi_offset);
+ /* Filter noise */
+ if (theta_dot <= NoiseLimit && theta_dot >= - NoiseLimit)
+ continue;
+ /* Teh magic constant (or not?) */
+ //theta -= theta_dot * -0.00307;
+ theta -= theta_dot * 0.003;
+ }
+ return;
+}
+
+
+task main ()
+{
+ float pwr = 0;
+ int full_steps = 0;
+
+ nMaxDataFileSize = 8000;
+ CreateDatalog (8000);
+
+ StartTask (AngleComputer);
+
+ /* Initialize motors */
+ nMotorEncoder[motorC] = 0;
+ nMotorEncoder[motorA] = 0;
+ /* This causes the motors to stop when they are set to zero */
+ bFloatDuringInactiveMotorPWM = false;
+
+ while (state != StateRunning)
+ wait1Msec (5);
+
+ while (true)
+ {
+ wait1Msec (dt);
+
+ //if (abs (theta) < 0.1)
+ // pwr = 0;
+ //else
+ //if (abs (theta) > 0.1)
+ {
+ if (theta > 0)
+ {
+ if (theta_dot > 0)
+ pwr = min (100, 12 + 10 * theta + 0.0 * theta_dot);
+ else
+ pwr = min (100, 12 + 10 * theta + 0.0 * theta_dot);
+ }
+ else
+ {
+ if (theta_dot > 0)
+ pwr = max (-100, -12 + 10 * theta + 0.0 * theta_dot);
+ else
+ pwr = max (-100, -12 + 10 * theta + 0.0 * theta_dot);
+ }
+ }
+ pwr *= 0.9;
+
+/*
+
+ if (abs (theta) < 0.1)
+ pwr = 0;
+ else if (abs (theta) < 2)
+ {
+ if (theta > 0)
+ pwr = 20;
+ else
+ pwr = - 20;
+ }
+ else
+ {
+ if (theta > 0)
+ pwr = 20*theta;
+ else
+ pwr = 20*theta;
+ }*/
+ /*
+ else if (abs (theta) < 5)
+ {
+ if (theta > 0)
+ pwr = 5 + k_ang * theta;
+ else
+ pwr = -5 + k_ang * theta;
+ }
+ else
+ {
+ if (theta > 0)
+ pwr = -40 + 2 * k_ang * theta;
+ else
+ pwr = 40 + 2 * k_ang * theta;
+ }*/
+
+
+ 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)
+ {
+ theta = 0;
+ full_steps = 0;
+ wait1Msec (post_timeout_dt);
+ }
+ }
+ StopAllTasks ();
+ return;
+}