Diff: main.cpp

Revision:

0:d96713a4739f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jun 01 08:17:40 2012 +0000
@@ -0,0 +1,540 @@
+#include "mbed.h"
+#include "EthernetNetIf.h"
+#include "TCPSocket.h"
+#include "ADXL345_I2C.h"
+#include "ITG3200.h"
+#include "IMUfilter.h"
+
+#define TCP_LISTENING_PORT 1337
+//Gravity at Earth's surface in m/s/s
+#define g0 9.812865328
+//Number of samples to average.
+#define SAMPLES 4
+//Number of samples to be averaged for a null bias calculation
+//during calibration.
+#define CALIBRATION_SAMPLES 128
+//Convert from radians to degrees.
+#define toDegrees(x) (x * 57.2957795)
+//Convert from degrees to radians.
+#define toRadians(x) (x * 0.01745329252)
+//ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
+#define GYROSCOPE_GAIN (1 / 14.375)
+//Full scale resolution on the ADXL345 is 4mg/LSB.
+#define ACCELEROMETER_GAIN (0.004 * g0)
+//Sampling gyroscope at 200Hz.
+#define GYRO_RATE   0.002
+//Sampling accelerometer at 200Hz.
+#define ACC_RATE    0.002
+//Updating filter at 100Hz.
+#define FILTER_RATE 0.01
+#define CORRECTION_MAGNITUDE 50
+
+
+//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);
+ADXL345_I2C accelerometer(p9, p10);
+I2C i2c(p9, p10); // sda, scl
+ITG3200 gyroscope(p9, p10);
+Ticker accelerometerTicker;
+Ticker gyroscopeTicker;
+Ticker filterTicker;
+Ticker dataTicker;
+Ticker algorithmTicker;
+DigitalOut led1(LED1, "led1");
+DigitalOut led2(LED2, "led2");
+DigitalOut led4(LED4, "led4");
+PwmOut m1(p21);
+PwmOut m2(p22);
+PwmOut m3(p23);
+PwmOut m4(p24);
+
+// EthernetNetIf eth;
+EthernetNetIf eth(
+  IpAddr(192,168,0,25), //IP Address
+  IpAddr(255,255,255,0), //Network Mask
+  IpAddr(192,168,0,1), //Gateway
+  IpAddr(192,168,0,1)  //DNS
+);
+
+TCPSocket ListeningSock;
+TCPSocket* pConnectedSock; // for ConnectedSock
+Host client;
+TCPSocketErr err;
+
+int calibrate   = 0,
+    calibrated  = 0,
+    start       = 0,
+    started     = 0,
+    alg_enable  = 0;
+    
+double a_motor_1 = 0,
+       a_motor_2 = 0,
+       a_motor_3 = 0,
+       a_motor_4 = 0;
+      
+float m1_set    = 0.001,
+      m2_set    = 0.001,
+      m3_set    = 0.001,
+      m4_set    = 0.001,
+      thrust_m1 = 3,
+      thrust_m2 = 3,
+      thrust_m3 = 3,
+      thrust_m4 = 3;
+
+char data[128];
+//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
+//or "biased" to 0.
+double w_xBias;
+double w_yBias;
+double w_zBias;
+
+//Offsets for the accelerometer.
+//Same as with the gyroscope.
+double a_xBias;
+double a_yBias;
+double a_zBias;
+
+//Accumulators used for oversampling and then averaging.
+volatile double a_xAccumulator = 0;
+volatile double a_yAccumulator = 0;
+volatile double a_zAccumulator = 0;
+volatile double w_xAccumulator = 0;
+volatile double w_yAccumulator = 0;
+volatile double w_zAccumulator = 0;
+
+//Accelerometer and gyroscope readings for x, y, z axes.
+volatile double a_x;
+volatile double a_y;
+volatile double a_z;
+volatile double w_x;
+volatile double w_y;
+volatile double w_z;
+
+//Buffer for accelerometer readings.
+int readings[3];
+//Number of accelerometer samples we're on.
+int accelerometerSamples = 0;
+//Number of gyroscope samples we're on.
+int gyroscopeSamples = 0;
+
+void onConnectedTCPSocketEvent(TCPSocketEvent e);
+void onListeningTCPSocketEvent(TCPSocketEvent e);
+void tcp_send(const char*);
+//Set up the ADXL345 appropriately.
+void initializeAcceleromter(void);
+//Calculate the null bias.
+void calibrateAccelerometer(void);
+//Take a set of samples and average them.
+void sampleAccelerometer(void);
+//Set up the ITG3200 appropriately.
+void initializeGyroscope(void);
+//Calculate the null bias.
+void calibrateGyroscope(void);
+//Take a set of samples and average them.
+void sampleGyroscope(void);
+//Update the filter and calculate the Euler angles.
+void filter(void);
+void getI2CAddress();
+
+void initializeAccelerometer(void) {
+
+    //Go into standby mode to configure the device.
+    accelerometer.setPowerControl(0x00);
+    //Full resolution, +/-16g, 4mg/LSB.
+    accelerometer.setDataFormatControl(0x0B);
+    //200Hz data rate.
+    accelerometer.setDataRate(ADXL345_200HZ);
+    //Measurement mode.
+    accelerometer.setPowerControl(0x08);
+    //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
+    wait_ms(22);
+
+}
+
+void sampleAccelerometer(void) {
+
+    //Have we taken enough samples?
+    if (accelerometerSamples == SAMPLES) {
+
+        //Average the samples, remove the bias, and calculate the acceleration
+        //in m/s/s.
+        a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
+        a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
+        a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
+
+        a_xAccumulator = 0;
+        a_yAccumulator = 0;
+        a_zAccumulator = 0;
+        accelerometerSamples = 0;
+
+    } else {
+        //Take another sample.
+        accelerometer.getOutput(readings);
+
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+
+        accelerometerSamples++;
+    }
+}
+
+void initializeGyroscope(void) {
+
+    //Low pass filter bandwidth of 42Hz.
+    gyroscope.setLpBandwidth(LPFBW_42HZ);
+    //Internal sample rate of 200Hz. (1kHz / 5).
+    gyroscope.setSampleRateDivider(4);
+
+}
+
+void calibrateGyroscope(void) {
+
+    w_xAccumulator = 0;
+    w_yAccumulator = 0;
+    w_zAccumulator = 0;
+
+    //Take a number of readings and average them
+    //to calculate the gyroscope bias offset.
+    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
+
+        w_xAccumulator += gyroscope.getGyroX();
+        w_yAccumulator += gyroscope.getGyroY();
+        w_zAccumulator += gyroscope.getGyroZ();
+        wait(GYRO_RATE);
+
+    }
+
+    //Average the samples.
+    w_xAccumulator /= CALIBRATION_SAMPLES;
+    w_yAccumulator /= CALIBRATION_SAMPLES;
+    w_zAccumulator /= CALIBRATION_SAMPLES;
+
+    w_xBias = w_xAccumulator;
+    w_yBias = w_yAccumulator;
+    w_zBias = w_zAccumulator;
+
+    w_xAccumulator = 0;
+    w_yAccumulator = 0;
+    w_zAccumulator = 0;
+
+}
+
+void sampleGyroscope(void) {
+
+    //Have we taken enough samples?
+    if (gyroscopeSamples == SAMPLES) {
+
+        //Average the samples, remove the bias, and calculate the angular
+        //velocity in rad/s.
+        w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
+        w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
+        w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
+
+        w_xAccumulator = 0;
+        w_yAccumulator = 0;
+        w_zAccumulator = 0;
+        gyroscopeSamples = 0;
+
+    } else {
+        //Take another sample.
+        w_xAccumulator += gyroscope.getGyroX();
+        w_yAccumulator += gyroscope.getGyroY();
+        w_zAccumulator += gyroscope.getGyroZ();
+
+        gyroscopeSamples++;
+
+    }
+
+}
+
+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 temp_x = toDegrees(imuFilter.getRoll());
+        int temp_y = toDegrees(imuFilter.getPitch());
+        
+        double x = temp_x;
+        double y = temp_y;
+        
+        a_motor_1   = thrust_m1 + ((((float)x/-2)-((float)y/2))/CORRECTION_MAGNITUDE);
+        thrust_m1   += ((((float)x/-2)-((float)y/2))/CORRECTION_MAGNITUDE)/20;
+        
+        if(a_motor_1 < 0.1){ 
+            m1_set = 0.001175;
+            a_motor_1 = 0.999;
+        }
+        else{m1_set = (a_motor_1+117.5)/100000;}
+  
+  
+        a_motor_2   = thrust_m2 + ((((float)x/-2)+((float)y/2))/CORRECTION_MAGNITUDE);
+        thrust_m2   += ((((float)x/-2)+((float)y/2))/CORRECTION_MAGNITUDE)/20;        
+        if(a_motor_2 < 0.1){ 
+            m2_set = 0.001175;
+            a_motor_2 = 0.999;
+        }
+        else{m2_set = (a_motor_2+117.5)/100000;}
+ 
+        a_motor_3   = thrust_m3 + ((((float)x/2)+((float)y/2))/CORRECTION_MAGNITUDE);
+        thrust_m3   += ((((float)x/2)+((float)y/2))/CORRECTION_MAGNITUDE)/20;
+        if(a_motor_3 < 0.1){ 
+            m3_set = 0.001175;
+            a_motor_3 = 0.999;
+        }
+        else{m3_set = (a_motor_3+117.5)/100000;}
+        
+        a_motor_4   = thrust_m4 + ((((float)x/2)-((float)y/2))/CORRECTION_MAGNITUDE);
+        thrust_m4   += ((((float)x/2)-((float)y/2))/CORRECTION_MAGNITUDE)/20;
+        if(a_motor_4 < 0.1){ 
+            m4_set = 0.001175;
+            a_motor_4 = 0.999;
+        }
+        else{m4_set = (a_motor_4+117.5)/100000;}
+        
+        m1.pulsewidth(m1_set);
+        m2.pulsewidth(m2_set);
+        m3.pulsewidth(m3_set);
+        m4.pulsewidth(m4_set);
+        alg_enable = 0;
+    }
+
+}
+
+void dataSender(void) {
+    char buffer [128];
+    sprintf (buffer, "x:%f | y:%f | am1:%f | am3:%f\r\n",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()),a_motor_1, a_motor_3);
+    tcp_send(buffer);
+}
+
+int main() {
+    EthernetErr ethErr = eth.setup();
+    if(ethErr){
+        printf("Error %d in setup.\r\n", ethErr);
+        return -1;
+    }
+    IpAddr ip = eth.getIp();
+    // Set the callbacks for Listening
+    ListeningSock.setOnEvent(&onListeningTCPSocketEvent);   
+    // bind and listen on TCP
+    err=ListeningSock.bind(Host(IpAddr(), TCP_LISTENING_PORT));
+    printf("Binding..\r\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);
+    wait(0.3);
+    
+    //Initialize inertial sensors.
+    initializeAccelerometer();
+    initializeGyroscope();
+
+                
+    Timer tmr;
+    tmr.start();
+    
+    while(true)
+    {
+        Net::poll();
+        if(tmr.read() > 0.2){
+            led4=!led4;
+            tmr.reset();
+        }
+        
+        if(calibrate && !calibrated){
+            calibrateAccelerometer();
+            calibrateGyroscope();
+            led2 = 1;
+            calibrated = 1;
+            tcp_send("Calibrated\r\n");
+        }
+        
+        if(calibrated && start && !started){
+            //Accelerometer data rate is 500Hz, so we'll sample at this speed.
+            accelerometerTicker.attach(&sampleAccelerometer, 0.002);
+            //Gyroscope data rate is 500Hz, so we'll sample at this speed.
+            gyroscopeTicker.attach(&sampleGyroscope, 0.002);
+            //Update the filter variables at the correct rate.
+            filterTicker.attach(&filter, FILTER_RATE);
+            dataTicker.attach(&dataSender, 0.2);
+            algorithmTicker.attach(&algorithm, 0.001);
+            started = 1;
+        }        
+    }  
+}
+
+void calibrateAccelerometer(void) {
+
+    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++) {
+
+        accelerometer.getOutput(readings);
+
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+
+        wait(ACC_RATE);
+
+    }
+
+    a_xAccumulator /= CALIBRATION_SAMPLES;
+    a_yAccumulator /= CALIBRATION_SAMPLES;
+    a_zAccumulator /= CALIBRATION_SAMPLES;
+
+    //At 4mg/LSB, 250 LSBs is 1g.
+    a_xBias = a_xAccumulator;
+    a_yBias = a_yAccumulator;
+    a_zBias = (a_zAccumulator - 250);
+
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+}
+
+void tcp_send( const char* data ){
+    int len = strlen(data);
+    pConnectedSock->send(data, len);
+}
+
+void onConnectedTCPSocketEvent(TCPSocketEvent e)
+{
+   switch(e)
+    {
+    case TCPSOCKET_CONNECTED:
+        led1 = 1;
+        printf("TCP Socket Connected\r\n");
+        break;
+    case TCPSOCKET_WRITEABLE:
+        led1 = 1;
+        //Can now write some data...
+        printf("TCP Socket Writable\r\n");
+        break;
+    case TCPSOCKET_READABLE:
+        led1 = 1;
+        //Can now read dome data...
+        printf("TCP Socket Readable\r\n");
+       // Read in any available data into the buffer
+       char buff[128];
+       while ( int len = pConnectedSock->recv(buff, 128) ) {
+       // And send straight back out again
+           //pConnectedSock->send(buff, len);
+           int test = buff[0];
+           char buffer[128];
+           sprintf(buffer, "|%i|\r\n", test);
+           tcp_send(buffer);
+           if(test == 99)   {calibrate = 1;}
+           if(test == 115)  {start = 1;}
+           buff[len]=0; // make terminater
+           printf("Received&Wrote:%s\r\n",buff);
+       }
+       break;
+    case TCPSOCKET_CONTIMEOUT:
+        printf("TCP Socket Timeout\r\n");
+        break;
+    case TCPSOCKET_CONRST:
+        printf("TCP Socket CONRST\r\n");
+        break;
+    case TCPSOCKET_CONABRT:
+        printf("TCP Socket CONABRT\r\n");
+        break;
+    case TCPSOCKET_ERROR:
+        printf("TCP Socket Error\r\n");
+        break;
+    case TCPSOCKET_DISCONNECTED:
+    //Close socket...
+        printf("TCP Socket Disconnected\r\n");        
+        pConnectedSock->close();
+        break;
+    default:
+        printf("DEFAULT\r\n"); 
+      }
+}
+
+void onListeningTCPSocketEvent(TCPSocketEvent e)
+{
+    switch(e)
+    {
+    case TCPSOCKET_ACCEPT:
+        printf("Listening: TCP Socket Accepted\r\n");
+        // Accepts connection from client and gets connected socket.   
+        err=ListeningSock.accept(&client, &pConnectedSock);
+        if (err) {
+            printf("onListeningTcpSocketEvent : Could not accept connection.\r\n");
+            return; //Error in accept, discard connection
+        }
+        // Setup the new socket events
+        pConnectedSock->setOnEvent(&onConnectedTCPSocketEvent);
+        // 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", 
+           clientIp[0], clientIp[1], clientIp[2], clientIp[3]);
+       break;
+    // the following cases will not happen
+    case TCPSOCKET_CONNECTED:
+        printf("Listening: TCP Socket Connected\r\n");
+        break;
+    case TCPSOCKET_WRITEABLE:
+        printf("Listening: TCP Socket Writable\r\n");
+        break;
+    case TCPSOCKET_READABLE:
+        printf("Listening: TCP Socket Readable\r\n");
+        break;
+    case TCPSOCKET_CONTIMEOUT:
+        printf("Listening: TCP Socket Timeout\r\n");
+        break;
+    case TCPSOCKET_CONRST:
+        printf("Listening: TCP Socket CONRST\r\n");
+        break;
+    case TCPSOCKET_CONABRT:
+        printf("Listening: TCP Socket CONABRT\r\n");
+        break;
+    case TCPSOCKET_ERROR:
+        printf("Listening: TCP Socket Error\r\n");
+        break;
+    case TCPSOCKET_DISCONNECTED:
+    //Close socket...
+        printf("Listening: TCP Socket Disconnected\r\n");        
+        ListeningSock.close();
+        break;
+    default:
+        printf("DEFAULT\r\n"); 
+     };
+}
+
+void getI2CAddress()
+{
+    int count = 1;
+    for (int address=0; address<256; address+=2) {
+        if (!i2c.write(address, NULL, 0)) { // 0 returned is ok    
+            char buffer [128];
+            sprintf (buffer, "%i: - %i\n",count, address);
+            tcp_send(buffer);
+            count++;    
+        }           
+    }
+}
\ No newline at end of file