summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorGuillaume Seguin <guillaume@segu.in>2008-03-09 23:30:06 +0100
committerGuillaume Seguin <guillaume@segu.in>2008-03-09 23:30:06 +0100
commit499bd4f2542abe7c7360d9baa6c0d01e4c6fd10c (patch)
tree3ba12a4a3fa4210f04d595c6528a96c6aae67f91
parent38a76f593fd56e7b0a0dc5971ec3b2a209b43ece (diff)
downloadsegway-499bd4f2542abe7c7360d9baa6c0d01e4c6fd10c.tar.gz
segway-499bd4f2542abe7c7360d9baa6c0d01e4c6fd10c.tar.bz2
* Attempt to use two gyros
-rw-r--r--segway.c10
1 files changed, 8 insertions, 2 deletions
diff --git a/segway.c b/segway.c
index e654216..b2ab0b1 100644
--- a/segway.c
+++ b/segway.c
@@ -43,6 +43,7 @@ typedef enum
GyroState state = StateWarmup;
float theta = 0;
+float theta2 = 0;
inline float min (float v1, float v2)
{
@@ -74,11 +75,16 @@ task AngleComputer ()
while (true)
{
wait1Msec (dt);
- r1 = SensorRaw (GyroSensor1) - SensorRaw (GyroSensor2);
- r2 = ((float) r1 - GyroOffset1 + GyroOffset2) / 2.0;
+ 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;
}
return;
}