Leo Veenstra
Dependencies: EthernetNetIf mbed
Revision 3:849ebcaa62c7, committed 2012-04-24
- Comitter:
- SED9008
- Date:
- Tue Apr 24 14:12:54 2012 +0000
- Parent:
- 2:53614df77a8e
- Commit message:
- sofar
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 53614df77a8e -r 849ebcaa62c7 main.cpp
--- a/main.cpp Thu Apr 19 10:25:48 2012 +0000
+++ b/main.cpp Tue Apr 24 14:12:54 2012 +0000
@@ -9,7 +9,7 @@
//Gravity at Earth's surface in m/s/s
#define g0 9.812865328
//Number of samples to average.
-#define SAMPLES 2
+#define SAMPLES 3
//Number of samples to be averaged for a null bias calculation
//during calibration.
#define CALIBRATION_SAMPLES 128
@@ -22,16 +22,18 @@
//Full scale resolution on the ADXL345 is 4mg/LSB.
#define ACCELEROMETER_GAIN (0.004 * g0)
//Sampling gyroscope at 200Hz.
-#define GYRO_RATE 0.0005
+#define GYRO_RATE 0.001
//Sampling accelerometer at 200Hz.
-#define ACC_RATE 0.0005
+#define ACC_RATE 0.001
//Updating filter at 40Hz.
#define FILTER_RATE 0.02
+#define CORRECTION_DELAY 10
+#define CORRECTION_MAGNITUDE 2
//At rest the gyroscope is centred around 0 and goes between about
//-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
//5/15 = 0.3 degrees/sec.
-IMUfilter imuFilter(FILTER_RATE, 0.3);
+IMUfilter imuFilter(FILTER_RATE, 7);
ADXL345_I2C accelerometer(p9, p10);
L3G4200D gyroscope(p9, p10);
@@ -43,11 +45,29 @@
Ticker accelerometerTicker;
Ticker gyroscopeTicker;
Ticker filterTicker;
+Ticker algorithmTicker;
-
+
+//Variables for the tcp controls
int connected = 0,
calibrated = 0,
- led1 = 0;
+ led1 = 0,
+ alg_enable = 0,
+ alg_cnt = 0,
+ thrust = 2,
+ last_m2 = 0,
+ last_m4 = 0;
+
+//stabilizing variables, these control the thrust and wanted x/y level
+float m1_set = 0.001,
+ m2_set = 0.001,
+ m3_set = 0.001,
+ m4_set = 0.001;
+
+double a_motor_1 = 0,
+ a_motor_2 = 0,
+ a_motor_3 = 0,
+ a_motor_4 = 0;
//Offsets for the gyroscope.
//The readings we take when the gyroscope is stationary won't be 0, so we'll
//average a set of readings we do get when the gyroscope is stationary and
@@ -101,6 +121,7 @@
void sampleGyroscope(void);
//Update the filter and calculate the Euler angles.
void filter(void);
+void algorithm(void);
void tcp_send(const char*);
void onConnectedTCPSocketEvent(TCPSocketEvent e);
void onListeningTCPSocketEvent(TCPSocketEvent e);
@@ -179,16 +200,62 @@
}
void filter(void) {
-
//Update the filter variables.
imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
//Calculate the new Euler angles.
imuFilter.computeEuler();
+ alg_enable = 1;
+}
+
+void algorithm(void)
+{
+ if(alg_enable){
+
+ int x = toDegrees(imuFilter.getRoll());
+ int y = toDegrees(imuFilter.getPitch());
+
+ a_motor_1 = thrust + (((-1*x)-y)/CORRECTION_MAGNITUDE+(((((float)x/-2)-((float)y/2))/CORRECTION_MAGNITUDE)*0.5*pow(alg_cnt,(0.3-(alg_cnt/(100+(5*alg_cnt)))))));
+ if(a_motor_1 < 1){
+ m1_set = 0.001175;
+ a_motor_1 = 0.999;
+ }
+ else{m1_set = (a_motor_1+117.5)/100000;}
+
+ a_motor_2 = thrust + ((x-y)/CORRECTION_MAGNITUDE+(((((float)x/2)-((float)y/2))/CORRECTION_MAGNITUDE)*0.5*pow(alg_cnt,(0.3-(alg_cnt/(100+(5*alg_cnt)))))));
+// if(a_motor_2>50){
+// m2_set = 0.0015;
+// a_motor_2 = 49.01;
+// }
+ if(a_motor_2 < 1){
+ m2_set = 0.001175;
+ a_motor_2 = 0.999;
+ }
+ else{m2_set = (a_motor_2+117.5)/100000;}
+
+ a_motor_3 = thrust + ((x+y)/CORRECTION_MAGNITUDE+(((((float)x/2)+((float)y/2))/CORRECTION_MAGNITUDE)*0.5*pow(alg_cnt,(0.3-(alg_cnt/(100+(5*alg_cnt)))))));
+ if(a_motor_3 < 1){
+ m3_set = 0.001175;
+ a_motor_3 = 0.999;
+ }
+ else{m3_set = (a_motor_3+117.5)/100000;}
+
+
+ a_motor_4 = thrust + ((y-x)/CORRECTION_MAGNITUDE+(((((float)y/2)-((float)x/2))/CORRECTION_MAGNITUDE)*0.5*pow(alg_cnt,(0.3-(alg_cnt/(100+(5*alg_cnt)))))));
+ if(a_motor_4 < 1){
+ m4_set = 0.001175;
+ a_motor_4 = 0.999;
+ }
+ else{m4_set = (a_motor_4+117.5)/100000;}
+
+
+ if(alg_cnt++ > 50){
+ alg_enable = 0;
+ alg_cnt = 0;
+ }
+ }
}
-
-
int main() {
EthernetErr ethErr = eth.setup();
@@ -202,6 +269,12 @@
if(err){printf("Binding Error\n");}
err=ListeningSock.listen(); // Starts listening
if(err){printf("Listening Error\r\n");}
+
+ m1.pulsewidth(0.001);
+ m2.pulsewidth(0.001);
+ m3.pulsewidth(0.001);
+ m4.pulsewidth(0.001);
+ wait(0.3);
//Initialize inertial sensors.
initializeAccelerometer();
@@ -210,12 +283,12 @@
//Set up timers.
//Accelerometer data rate is 200Hz, so we'll sample at this speed.
- accelerometerTicker.attach(&sampleAccelerometer, 0.005);
+// accelerometerTicker.attach(&sampleAccelerometer, 0.005);
//Gyroscope data rate is 200Hz, so we'll sample at this speed.
- gyroscopeTicker.attach(&sampleGyroscope, 0.005);
+// gyroscopeTicker.attach(&sampleGyroscope, 0.005);
//Update the filter variables at the correct rate.
- filterTicker.attach(&filter, FILTER_RATE);
-
+// filterTicker.attach(&filter, 0.02);
+
Timer tmr;
tmr.start();
@@ -240,6 +313,7 @@
gyroscopeTicker.attach(&sampleGyroscope, 0.005);
//Update the filter variables at the correct rate.
filterTicker.attach(&filter, FILTER_RATE);
+ algorithmTicker.attach(&algorithm, 0.0005);
tcp_send("Done initializing\r\n");
@@ -247,10 +321,12 @@
if(calibrated){
char buffer [128];
- sprintf (buffer, "x:%f - y:%f - z:%f \r\n",toDegrees(imuFilter.getRoll()),
- toDegrees(imuFilter.getPitch()),
- toDegrees(imuFilter.getYaw()));
+ sprintf (buffer, "x:%f | y:%f | am2:%f | am4:%f\r\n",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()),a_motor_2,a_motor_4);
tcp_send(buffer);
+ m1.pulsewidth(m1_set);
+ m2.pulsewidth(m2_set);
+ m3.pulsewidth(m3_set);
+ m4.pulsewidth(m4_set);
}
}
}
@@ -369,7 +445,7 @@
sprintf(buffer, "|%i|\r\n", test);
tcp_send(buffer);
if(test == 115) {connected = 1;}
-/* if(test == 49) {m1_set += 0.00001;}
+ if(test == 49) {m1_set += 0.00001;}
if(test == 50) {m2_set += 0.00001;}
if(test == 51) {m3_set += 0.00001;}
if(test == 52) {m4_set += 0.00001;}
@@ -381,7 +457,7 @@
m1_set = 0.0011;
m2_set = 0.0011;
m3_set = 0.0011;
- m4_set = 0.0011;}*/
+ m4_set = 0.0011;}
buff[len]=0; // make terminater