summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorGuillaume Seguin <guillaume@segu.in>2008-03-09 23:31:16 +0100
committerGuillaume Seguin <guillaume@segu.in>2008-03-09 23:31:16 +0100
commit9a30e2288eb8cbfa896d01ef732bf979099dfa20 (patch)
treecbe5d3abe4306f67fc18b693c8157071a4903f79
parent499bd4f2542abe7c7360d9baa6c0d01e4c6fd10c (diff)
downloadsegway-9a30e2288eb8cbfa896d01ef732bf979099dfa20.tar.gz
segway-9a30e2288eb8cbfa896d01ef732bf979099dfa20.tar.bz2
* Various changes, mostly minor stuff (revert back to one gyro)
-rw-r--r--segway.c99
1 files changed, 53 insertions, 46 deletions
diff --git a/segway.c b/segway.c
index b2ab0b1..b215526 100644
--- a/segway.c
+++ b/segway.c
@@ -1,29 +1,15 @@
-/*
- * 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 NoiseLimit 0
+
+#define dt 3
+#define offset_dt 40
+
+#define k_offset 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
@@ -35,6 +21,9 @@ const tSensors SonarSensor = (tSensors) S3; /* Our sonar sensor */
#define timeout 300
+#define log_dt 10
+#define log_samples 2000
+
typedef enum
{
StateWarmup,
@@ -44,7 +33,9 @@ GyroState state = StateWarmup;
float theta = 0;
float theta2 = 0;
+float gyro_offset = 606;
+/* Helper functions */
inline float min (float v1, float v2)
{
if (v1 > v2)
@@ -61,39 +52,53 @@ inline float max (float v1, float v2)
return v2;
}
+task OffsetHandler ()
+{
+ while (true)
+ {
+ wait1Msec (offset_dt);
+ gyro_offset = (1.0 - k_offset) * gyro_offset + k_offset * ((float) SensorRaw (GyroSensor1));
+ }
+ return;
+}
+
+/* Detached thread that computes the current angle based on data from Gyros */
task AngleComputer ()
{
- float r1 = 0, r2 = 0;
+ float raw = 0, theta_dot = 0, old_theta_dot;
/* Increase execution priority */
nSchedulePriority = kHighPriority - 1;
+ SensorType[GyroSensor1] = sensorLightInactive;
wait1Msec (2000); /* Startup delay */
+ StartTask (OffsetHandler);
state = StateRunning;
while (true)
{
wait1Msec (dt);
- r1 = SensorRaw (GyroSensor1);
- r2 = (float) r1 - GyroOffset1;
- if (r2 <= NoiseLimit && r2 >= - NoiseLimit)
- r2 = 0;
- theta += + r2 * -0.00307;
- r1 = SensorRaw (GyroSensor2);
- r2 = (float) r1 - GyroOffset2;
- if (r2 <= NoiseLimit && r2 >= - NoiseLimit)
- r2 = 0;
- theta2 += + r2 * -0.00307;
+ /* Compute the angle with data given by first Gyro */
+ raw = SensorRaw (GyroSensor1);
+ /* Offset it correctly */
+ theta_dot = (float) raw - gyro_offset;
+ /* Filter noise */
+ if (theta_dot <= NoiseLimit && theta_dot >= - NoiseLimit)
+ theta_dot = 0;
+ theta_dot = old_theta_dot * (1 - k_tdot) + k_tdot * theta_dot;
+ old_theta_dot = theta_dot;
+ /* Teh magic constant (or not?) */
+ theta += theta_dot * -0.00307;
}
return;
}
+
task main ()
{
- float theta = 0;
- float theta_bias = 0;
float pwr = 0;
+ int d;
int full_steps = 0;
StartTask (AngleComputer);
@@ -110,23 +115,25 @@ task main ()
while (true)
{
wait1Msec (dt);
- theta_bias = theta_bias * (1.0 - k_bias) + theta * k_bias;
- pwr = theta - theta_bias;
+ pwr = theta;
if (abs (pwr) < 0.01)
- pwr = 0;
- else if (abs (pwr) < 2)
+ pwr = 0;
+ else if (abs (pwr) < 5)
{
if (pwr > 0)
- pwr = min (20, 8 + k_ang * pwr);
+ pwr = min (40, 15 + k_ang * pwr);
else
- pwr = max (-20, -8 + k_ang * pwr);
+ pwr = max (-40, -15 + k_ang * pwr);
}
- else if (pwr < 0)
- pwr = - 200;
else
- pwr = 200;
+ {
+ if (pwr < 0)
+ pwr = - 200;
+ else
+ pwr = 200;
+ }
motor[motorA] = pwr - k_sync * (nMotorEncoder[motorA]
- nMotorEncoder[motorC]);
@@ -141,7 +148,7 @@ task main ()
{
motor[motorA] = 0;
motor[motorC] = 0;
- break;
+ //break;
}
}
StopAllTasks ();