Diff: main.cpp

Revision:

1:6e34363efa95

Parent:

0:3cbe4eec57ae

--- a/main.cpp	Wed Mar 30 09:46:55 2011 +0000
+++ b/main.cpp	Sat Sep 17 09:11:16 2011 +0000
@@ -1,5 +1,7 @@
 #include "mbed.h"
 
+#define VERSION "0.0.2"
+
 DigitalOut motor1A(p9);
 DigitalOut motor1B(p10);
 DigitalOut motor2A(p11);
@@ -9,49 +11,56 @@
 DigitalOut motor4A(p15);
 DigitalOut motor4B(p16);
 
-DigitalOut digital1(p17);
-DigitalOut digital2(p18);
-DigitalOut digital3(p19);
-DigitalOut digital4(p20);
-
-SPI spi(p5, p6, p7); // mosi(out), miso(in), sclk(clock)
-DigitalOut cs(p8); // cs(select)
-
 PwmOut motor1(p21);
 PwmOut motor2(p22);
 PwmOut motor3(p23);
 PwmOut motor4(p24);
 
-DigitalOut led(LED4);
+DigitalOut digital1(p17);
+DigitalOut digital2(p18);
+DigitalOut digital3(p19);
+DigitalOut digital4(p20);
+DigitalOut led(LED1);
+
+SPI spi(p5, p6, p7); // mosi(out), miso(in), sclk(clock)
+DigitalOut cs(p8); // cs(select)
 
 Serial pc(USBTX, USBRX);
-
-uint8_t* values;
-uint8_t* location;
-
-char buffer[3];
-int i, inValue, bufferIndex;
+// for latency calculation and transmission count
+Timer timer;
+unsigned long a, temp, count;
+// for enableing disableing autosending sensor values
+bool autosend;
+// the analog sensor values for ADC
+volatile uint8_t* values;
+volatile uint8_t* location;
+// the serial message buffer
+volatile char command[3];
+volatile int index;
 
 // maps the value x to a new given range
-double map( double x, double in_min, double in_max, double out_min, double out_max ) {
-    return ( x - in_min ) * ( out_max - out_min ) / ( in_max - in_min ) + out_min;
+double map(double x, double in_min, double in_max, double out_min, double out_max) 
+{
+    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
 
-// shifts 2 8 bit integers into one integer
-int getIntValue( uint8_t high, uint8_t low ) {
-    return ( high << 8 ) | low;
+// shifts 2 8 bit bytes into one integer
+int getIntValue( uint8_t high, uint8_t low ) 
+{
+    return (high << 8) | low;
 }
 
 // sets the given pwm value
-void setPWM( int value, PwmOut pwm, DigitalOut a, DigitalOut b ) {
+void setPWM(int value, PwmOut pwm, DigitalOut a, DigitalOut b) 
+{
     // block the motors
-    if ( value > 511 ) {
+    if (value > 511) {
         a = 0;
         b = 0;
         pwm = 1;
     } else {
         // clockwise run
-        if ( value > 255 ) {
+        if (value > 255) {
             value = value - 256;
             a = 0;
             b = 1;
@@ -60,81 +69,118 @@
             b = 0;
         }
         // map the 0-255 value to 0-1
-        pwm = map( value, 0, 255, 0, 1 );
+        pwm = map(value, 0, 255, 0, 1);
     }
 }
 
 // specifies the action for the user input
-void specifyAction( char action, int value ) {
+void specifyAction(char action, int value)
+{
     // specify the action
-    switch ( action ) {
+    switch (action) {
+        case 40:
+            // print the version of this software
+            pc.printf("%s",VERSION);
+            pc.putc(255);
+            break;
+        case 41:
+            // enable or disable autosending sensor values
+            autosend = value;
+            break;
+        case 42:
+            // send last command latency
+            pc.putc(temp >> 8);
+            pc.putc(temp);
+            // the end of the message
+            pc.putc(255);
         case 44:
             // print the analogs and digitals
+            // the sensor reading checksum
+            char checksum = 0;
+            // restart pointer
             values = location;
-            for ( int i=0; i<16; i++ ) {
-                pc.putc( *( values++ ) );
+            // print analog sensor values
+            for(int i = 0; i < 16; i++) {
+                pc.putc(*(values));
+                // add analog sensor to the checksum                
+                checksum ^= *(values++);
             }
-            
-            pc.putc( '\0' );
-            pc.putc( 13 );
+            // add digital sensors to the checksum
+            checksum ^= '\0';
+            // print digital sensor values
+            pc.putc('\0');
+            // print checksum
+            pc.putc(checksum);
+            // print last latency value
+            pc.putc(temp >> 8);
+            pc.putc(temp);
+            // send packet count
+            count++;
+            pc.putc(count >> 16);
+            pc.putc(count >> 8);
+            pc.putc(count);
+            // the end of the message
+            pc.putc(255);
             break;
         case 162:
             // set the first PWMs value
-            setPWM( value, motor1, motor1A, motor1B );
+            setPWM(value, motor1, motor1A, motor1B);
             break;
         case 163:
             // set the second PWMs value
-            setPWM( value, motor2, motor2A, motor2B );
+            setPWM(value, motor2, motor2A, motor2B);
             break;
         case 164:
             // set the third PWMs value
-            setPWM( value, motor3, motor3A, motor3B );
+            setPWM(value, motor3, motor3A, motor3B);
             break;
         case 165:
             // set the fourth PWMs value
-            setPWM( value, motor4, motor4A, motor4B );
+            setPWM(value, motor4, motor4A, motor4B);
             break;
         case 230:
             // Goto _reset ?
             break;
         case 231:
             // enable or disable servos
-            if ( value == 1 ) {
+            if (value == 1) {
                 // enable servo ?
-            } else if ( value == 2 ) {
+            } else if (value == 2) {
                 // disable servo ?
             }
             break;
         case 233:
             // set digitals values
-            digital1 = ( value & 1 );
-            digital2 = ( ( value >> 1 ) & 1 );
-            digital3 = ( ( value >> 2 ) & 1 );
-            digital4 = ( ( value >> 3 ) & 1 );
+            digital1 = (value & 1);
+            digital2 = ((value >> 1) & 1);
+            digital3 = ((value >> 2) & 1);
+            digital4 = ((value >> 3) & 1);
             break;
         default:
-            //pc.printf( "default\n" );
+            pc.printf("unspecified command\n");
     }
 }
 
-int readADC( int channel, DigitalOut select ) {
-    // select the device
-    select = 0;
-    
+// for reading external ADC values
+int readSensor(int channel, DigitalOut select)
+{  
     // sending the 5 bits + 2 bits to ignore the null bits
     // coming from the device, so the data that is sent is 11000101
     uint8_t commandbits = 0xC0;
-    commandbits |= ( channel << 2 );
-    spi.write( commandbits );
-
+    commandbits |= (channel << 2);
+    
+    // select the device
+    select = 0;
+    // send command bits
+    spi.write(commandbits);
     // now the device sends back the readings 12 bits, 7 bits at a time
     uint8_t high = spi.write(0x0);
     uint8_t low = spi.write(0x0);
     low = low >> 2;
 
     // shift to get the high and low byte
-    high = ( high >> 3 );
-    low = ( high << 5 ) | low;
+    high = (high >> 3);
+    low = ( high << 5) | low;
 
     // deselect the device
     select = 1;
@@ -146,30 +192,38 @@
     return 0;
 }
 
-void serialEvent() {
+// serial interrupt method
+void serialEvent() 
+{
+    // if we are getting a new message
+    if(!index) {
+        // mark the time
+        a = timer.read_us();
+    }
     char c = pc.getc();
     // if we received enter
-    if( c == 13 ) {
-        //pc.printf( "char=[%c] value=[%u]", buffer[0],getIntValue(buffer[1],buffer[2]) );
-        specifyAction( buffer[ 0 ], getIntValue( buffer[ 1 ], buffer[ 2 ] ) );
-        buffer[ 0 ] = '\0';
-        bufferIndex = 0;
+    if( c == 255 ) {
+        specifyAction(command[ 0 ], getIntValue(command[ 1 ], command[ 2 ]));
+        index = 0;
+        // calculate the latency
+        temp = timer.read_us() - a;
     }
+    // otherwise add byte to buffer
     else {
-        buffer[ bufferIndex++ ] = c;
+        command[index++] = c;
     }
 }
 
 // the main method
-int main() {
+int main() 
+{
     // Setup the spi for 7 bit data, high steady state clock,
     // second edge capture, with a 1MHz clock rate
     spi.format(7,0);
     spi.frequency(1000000);
     
-    // specify the usb baud rate, supported are :
-    // 110, 300, 600, 1200, 2400, 4800, 9600, 14400, 19200, 38400, 57600, 115200, 230400, 460800, 921600
-    pc.baud( 921600 );
+    // specify the usb baud rate
+    pc.baud(115200);
 
     // allocate 16 bytes of space for the adc values
     values = (uint8_t*) malloc(16);
@@ -177,19 +231,20 @@
     location = values;
 
     // attach a serial interrupt
-    pc.attach( &serialEvent );
+    pc.attach(&serialEvent);
 
     // to indicate programm is running
     led = 1;
 
+    timer.start();
     // endless loop
-    while ( true ) {
-        // start from the beginning
+    while (true) {
         values = location;
-        // read the ADC values
-        for ( i = 0; i < 8; i++ ) {
-            readADC( i, cs );
+        for (int i=0; i<8; i++) {
+            readSensor(i, cs);
         }
+        if(autosend)
+            specifyAction(44,0);
     }
 }