Diff: main.cpp

Revision:

2:53614df77a8e

Parent:

1:5721a5772035

Child:

3:849ebcaa62c7

--- a/main.cpp	Thu Apr 12 15:46:38 2012 +0000
+++ b/main.cpp	Thu Apr 19 10:25:48 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 4
+#define SAMPLES 2
 //Number of samples to be averaged for a null bias calculation
 //during calibration.
 #define CALIBRATION_SAMPLES 128
@@ -22,13 +22,11 @@
 //Full scale resolution on the ADXL345 is 4mg/LSB.
 #define ACCELEROMETER_GAIN (0.004 * g0)
 //Sampling gyroscope at 200Hz.
-#define GYRO_RATE   0.005
+#define GYRO_RATE   0.0005
 //Sampling accelerometer at 200Hz.
-#define ACC_RATE    0.005
-//Updating filter at 20Hz.
-#define FILTER_RATE 0.025
-//Defines the delay for the corrections made by the algorithm
-#define CORRECTION_DELAY 5
+#define ACC_RATE    0.0005
+//Updating filter at 40Hz.
+#define FILTER_RATE 0.02
 
 //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
@@ -45,34 +43,12 @@
 Ticker accelerometerTicker;
 Ticker gyroscopeTicker;
 Ticker filterTicker;
-Ticker motorTicker;
 
-//Variables for the tcp controls
+
 int connected   = 0,
     calibrated  = 0,
     led1        = 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;
-
-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.
+//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
 //take those away from subsequent readings to ensure the gyroscope is offset
@@ -125,7 +101,6 @@
 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);
@@ -212,38 +187,22 @@
 
 }
 
-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() {
-   
+
     EthernetErr ethErr = eth.setup();
     if(ethErr)  { return -1;}
-    IpAddr ip = eth.getIp();    
+    IpAddr ip = eth.getIp();
     // Set the callbacks for Listening
-    ListeningSock.setOnEvent(&onListeningTCPSocketEvent);     
+    ListeningSock.setOnEvent(&onListeningTCPSocketEvent);
     // bind and listen on TCP
     err=ListeningSock.bind(Host(IpAddr(), TCP_LISTENING_PORT));
     //Deal with that error...
-    if(err){printf("Binding Error\n");} 
+    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); 
-    
+
     //Initialize inertial sensors.
     initializeAccelerometer();
     calibrateAccelerometer();
@@ -251,34 +210,29 @@
 
     //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(0.2);
-        
+        wait(FILTER_RATE);
+
         Net::poll();
         if(tmr.read() > 0.2){
 //            led4=!led4; //Show that we are alive
             tmr.reset();
-        } 
+        }
         if(connected & led1 == 0){
             tcp_send("Connected\r\n");
             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);
@@ -286,44 +240,19 @@
             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);   
-            sprintf (buffer, "am1:%f\r\n",a_motor_1);
-            tcp_send(buffer);   
+            tcp_send(buffer);
         }
-    }  
+    }
 }
 
 void initializeAccelerometer(void) {
@@ -346,7 +275,7 @@
     a_xAccumulator = 0;
     a_yAccumulator = 0;
     a_zAccumulator = 0;
-    
+
     //Take a number of readings and average them
     //to calculate the zero g offset.
     for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
@@ -437,54 +366,23 @@
 //           pConnectedSock->send(buff, len);
            int test = buff[0];
            char buffer[128];
-           sprintf(buffer, "|%i|", test);
-           tcp_send(buffer); 
+           sprintf(buffer, "|%i|\r\n", test);
+           tcp_send(buffer);
            if(test == 115)  {connected = 1;}
-           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 == 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 == 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
            printf("Received&Wrote:%s\r\n",buff);
@@ -504,11 +402,11 @@
         break;
     case TCPSOCKET_DISCONNECTED:
     //Close socket...
-        printf("TCP Socket Disconnected\r\n");        
+        printf("TCP Socket Disconnected\r\n");
         pConnectedSock->close();
         break;
     default:
-        printf("DEFAULT\r\n"); 
+        printf("DEFAULT\r\n");
       }
 }
 
@@ -519,7 +417,7 @@
     {
     case TCPSOCKET_ACCEPT:
         printf("Listening: TCP Socket Accepted\r\n");
-        // Accepts connection from client and gets connected socket.   
+        // Accepts connection from client and gets connected socket.
         err=ListeningSock.accept(&client, &pConnectedSock);
         if (err) {
             printf("onListeningTcpSocketEvent : Could not accept connection.\r\n");
@@ -530,7 +428,7 @@
         // We can find out from where the connection is coming by looking at the
         // Host parameter of the accept() method
         IpAddr clientIp = client.getIp();
-        printf("Listening: Incoming TCP connection from %d.%d.%d.%d\r\n", 
+        printf("Listening: Incoming TCP connection from %d.%d.%d.%d\r\n",
            clientIp[0], clientIp[1], clientIp[2], clientIp[3]);
        break;
     // the following cases will not happen
@@ -557,10 +455,10 @@
         break;
     case TCPSOCKET_DISCONNECTED:
     //Close socket...
-        printf("Listening: TCP Socket Disconnected\r\n");        
+        printf("Listening: TCP Socket Disconnected\r\n");
         ListeningSock.close();
         break;
     default:
-        printf("DEFAULT\r\n"); 
+        printf("DEFAULT\r\n");
      };
 }
\ No newline at end of file