Leo Veenstra
Dependencies: EthernetNetIf mbed
Diff: main.cpp
- Revision:
- 1:5721a5772035
- Parent:
- 0:ad88907cf227
- Child:
- 2:53614df77a8e
--- a/main.cpp Tue Apr 03 17:46:32 2012 +0000
+++ b/main.cpp Thu Apr 12 15:46:38 2012 +0000
@@ -25,8 +25,10 @@
#define GYRO_RATE 0.005
//Sampling accelerometer at 200Hz.
#define ACC_RATE 0.005
-//Updating filter at 40Hz.
-#define FILTER_RATE 0.1
+//Updating filter at 20Hz.
+#define FILTER_RATE 0.025
+//Defines the delay for the corrections made by the algorithm
+#define CORRECTION_DELAY 5
//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
@@ -43,12 +45,34 @@
Ticker accelerometerTicker;
Ticker gyroscopeTicker;
Ticker filterTicker;
+Ticker motorTicker;
-
+//Variables for the tcp controls
int connected = 0,
calibrated = 0,
led1 = 0;
-//Offsets for the gyroscope.
+
+//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;
+
+int x_level_1 = 0,
+ x_level_2 = 0,
+ x_level_3 = 0,
+ x_level_4 = 0,
+ y_level_1 = 0,
+ y_level_2 = 0,
+ y_level_3 = 0,
+ y_level_4 = 0;
+
+//O1ffsets 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
//take those away from subsequent readings to ensure the gyroscope is offset
@@ -101,6 +125,7 @@
void sampleGyroscope(void);
//Update the filter and calculate the Euler angles.
void filter(void);
+void motor(void);
void tcp_send(const char*);
void onConnectedTCPSocketEvent(TCPSocketEvent e);
void onListeningTCPSocketEvent(TCPSocketEvent e);
@@ -187,7 +212,18 @@
}
-
+void motor(void) {
+ int x = toDegrees(imuFilter.getRoll());
+ int y = toDegrees(imuFilter.getPitch());
+ a_motor_1 = a_motor_1+((float)(x/2)/CORRECTION_DELAY)+((float)(y/2)/CORRECTION_DELAY);
+ if(a_motor_1>99){ m1_set = 0.002;}
+ else if(a_motor_1 < 1){ m1_set = 0.001175;}
+ else{ m1_set = (a_motor_1+117.5)/100000;}
+ m1.pulsewidth(m1_set);
+ m2.pulsewidth(m2_set);
+ m3.pulsewidth(m3_set);
+ m4.pulsewidth(m4_set);
+}
int main() {
@@ -203,6 +239,11 @@
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);
+
//Initialize inertial sensors.
initializeAccelerometer();
calibrateAccelerometer();
@@ -210,18 +251,18 @@
//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, FILTER_RATE);
Timer tmr;
tmr.start();
while(true)
{
- wait(FILTER_RATE);
+ wait(0.2);
Net::poll();
if(tmr.read() > 0.2){
@@ -233,6 +274,11 @@
led1 = 1;
calibrated = 1;
+// m1_set = 0.001175;
+// m2_set = 0.001175;
+// m3_set = 0.001175;
+// m4_set = 0.001175;
+
//Set up timers.
//Accelerometer data rate is 200Hz, so we'll sample at this speed.
accelerometerTicker.attach(&sampleAccelerometer, 0.005);
@@ -240,17 +286,42 @@
gyroscopeTicker.attach(&sampleGyroscope, 0.005);
//Update the filter variables at the correct rate.
filterTicker.attach(&filter, FILTER_RATE);
+
+ motorTicker.attach(&motor, 0.02);
tcp_send("Done initializing\r\n");
}
if(calibrated){
+
+
+
+
+
+/* a_motor_2 = a_motor_2-((((x_level_2-x)/2)-((y_level_2-y)/2))/CORRECTION_DELAY);
+ if(a_motor_2>99){ m2_set = (100+117.5)/100000;}
+ else if(a_motor_2 < 1){ m2_set = (0+117.5)/100000;}
+ else{ m2_set = (a_motor_2+117.5)/100000;}
+
+ a_motor_3 = a_motor_3+((((x_level_3-x)/2)-((y_level_3-y)/2))/CORRECTION_DELAY);
+ if(a_motor_3>99){ m3_set = (100+117.5)/100000;}
+ else if(a_motor_3 < 1){ m3_set = (0+117.5)/100000;}
+ else{ m3_set = (a_motor_3+117.5)/100000;}
+
+ a_motor_4 = a_motor_4+((((x_level_4-x)/2)+((y_level_4-y)/2))/CORRECTION_DELAY);
+ if(a_motor_4>99){ m4_set = (100+117.5)/100000;}
+ else if(a_motor_4 < 1){ m4_set = (0+117.5)/100000;}
+ else{ m4_set = (a_motor_4+117.5)/100000;}*/
+
+
char buffer [128];
sprintf (buffer, "x:%f - y:%f - z:%f \r\n",toDegrees(imuFilter.getRoll()),
toDegrees(imuFilter.getPitch()),
toDegrees(imuFilter.getYaw()));
- tcp_send(buffer);
+ tcp_send(buffer);
+ sprintf (buffer, "am1:%f\r\n",a_motor_1);
+ tcp_send(buffer);
}
}
}
@@ -366,22 +437,53 @@
// pConnectedSock->send(buff, len);
int test = buff[0];
char buffer[128];
- sprintf(buffer, "|%i|\r\n", test);
+ sprintf(buffer, "|%i|", test);
tcp_send(buffer);
if(test == 115) {connected = 1;}
-/* 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;}
- if(test == 113) {m1_set -= 0.00001;}
- if(test == 119) {m2_set -= 0.00001;}
- if(test == 101) {m3_set -= 0.00001;}
- if(test == 114) {m4_set -= 0.00001;}
+ if(test == 112) {
+ a_motor_1 += 5;
+ a_motor_1 += 5;
+ a_motor_1 += 5;
+ a_motor_1 += 5;
+ }
+ if(test == 49) {
+ m1_set += 0.00002;
+ sprintf (buffer, " %f\r\n",m1_set);
+ tcp_send(buffer);
+ }
+ if(test == 50) {
+ m2_set += 0.00002;
+ sprintf (buffer, " %f\r\n",m2_set);
+ tcp_send(buffer);
+ }
+ if(test == 51) {
+ m3_set += 0.00002;
+ sprintf (buffer, " %f\r\n",m3_set);
+ tcp_send(buffer);
+ }
+ if(test == 52) {
+ m4_set += 0.00002;
+ sprintf (buffer, " %f\r\n",m4_set);
+ tcp_send(buffer);;
+ }
+ if(test == 113) {
+ m1_set -= 0.00002;
+ }
+ if(test == 119) {
+ m2_set -= 0.00002;
+ }
+ if(test == 101) {
+ m3_set -= 0.00002;
+ }
+ if(test == 114) {
+ m4_set -= 0.00002;
+ }
if(test == 107) {
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