summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorGuillaume Seguin <guillaume@segu.in>2008-03-09 23:31:53 +0100
committerGuillaume Seguin <guillaume@segu.in>2008-03-09 23:31:53 +0100
commit72db83894a2337880ff02a0cd1254a246268da86 (patch)
treefdab3d8bd7b2b3b75b1c06a1848bf4cd16c816f2
parent9a30e2288eb8cbfa896d01ef732bf979099dfa20 (diff)
downloadsegway-72db83894a2337880ff02a0cd1254a246268da86.tar.gz
segway-72db83894a2337880ff02a0cd1254a246268da86.tar.bz2
* Further tests with one gyros (try to use average terms for offset, theta_dot...)
-rwxr-xr-x[-rw-r--r--]segway.c104
1 files changed, 82 insertions, 22 deletions
diff --git a/segway.c b/segway.c
index b215526..92c0148 100644..100755
--- a/segway.c
+++ b/segway.c
@@ -5,7 +5,8 @@ const tSensors GyroSensor1 = (tSensors) S1; /* Our first gyro sensor */
#define dt 3
#define offset_dt 40
-#define k_offset 0.2
+#define k_offset 0.001
+#define k_offset2 0.2
#define k_tdot 0.2
#define k_pos 0.05
@@ -19,7 +20,7 @@ const tSensors GyroSensor1 = (tSensors) S1; /* Our first gyro sensor */
#define min_dist 30
-#define timeout 300
+#define timeout 100
#define log_dt 10
#define log_samples 2000
@@ -32,7 +33,6 @@ typedef enum
GyroState state = StateWarmup;
float theta = 0;
-float theta2 = 0;
float gyro_offset = 606;
/* Helper functions */
@@ -52,11 +52,18 @@ inline float max (float v1, float v2)
return v2;
}
+float theta_dot = 0;
task OffsetHandler ()
{
while (true)
{
wait1Msec (offset_dt);
+ /*AddToDatalog (theta * 1000);
+ AddToDatalog (theta_dot * 1000);*/
+ //if (abs ((float) SensorRaw (GyroSensor1) - gyro_offset) > 1)
+ // continue;
+ if (abs (theta_dot) > 10)
+ continue;
gyro_offset = (1.0 - k_offset) * gyro_offset + k_offset * ((float) SensorRaw (GyroSensor1));
}
return;
@@ -65,31 +72,43 @@ task OffsetHandler ()
/* Detached thread that computes the current angle based on data from Gyros */
task AngleComputer ()
{
- float raw = 0, theta_dot = 0, old_theta_dot;
+ float raw = 0, old_theta_dot = 0, old_theta = 0, delta = 0;
/* Increase execution priority */
nSchedulePriority = kHighPriority - 1;
SensorType[GyroSensor1] = sensorLightInactive;
- wait1Msec (2000); /* Startup delay */
- StartTask (OffsetHandler);
+ wait1Msec (1000); /* Startup delay */
+ gyro_offset = SensorRaw (GyroSensor1);
+ for (int i = 0; i < 500; i++)
+ {
+ wait1Msec (dt);
+ /* Compute the angle with data given by first Gyro */
+ gyro_offset = (1.0 - k_offset2) * gyro_offset + k_offset2 * ((float) SensorRaw (GyroSensor1));
+ }
+ //StartTask (OffsetHandler);
state = StateRunning;
while (true)
{
wait1Msec (dt);
+ gyro_offset = (1.0 - k_offset) * gyro_offset + k_offset * ((float) SensorRaw (GyroSensor1));
/* 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;
+ continue;
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;
+ //theta -= theta_dot * -0.00307;
+ delta = theta_dot * 0.003 -0.005*old_theta;
+ //if (abs (theta) < abs (delta) && )
+ theta += theta_dot * 0.002 -0.00*old_theta;
+ old_theta = theta;
}
return;
}
@@ -98,9 +117,12 @@ task AngleComputer ()
task main ()
{
float pwr = 0;
- int d;
int full_steps = 0;
+
+ nMaxDataFileSize = 8000;
+ CreateDatalog (8000);
+
StartTask (AngleComputer);
/* Initialize motors */
@@ -116,31 +138,69 @@ task main ()
{
wait1Msec (dt);
- pwr = theta;
+ //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 *= 1.3;
+
+/*
- if (abs (pwr) < 0.01)
- pwr = 0;
- else if (abs (pwr) < 5)
+ 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 (pwr > 0)
- pwr = min (40, 15 + k_ang * pwr);
+ if (theta > 0)
+ pwr = 5 + k_ang * theta;
else
- pwr = max (-40, -15 + k_ang * pwr);
+ pwr = -5 + k_ang * theta;
}
else
{
- if (pwr < 0)
- pwr = - 200;
- else
- pwr = 200;
- }
+ 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)
+ if (pwr >= 100 || pwr <= - 100)
full_steps += 1;
else
full_steps = 0;