Diff: main.cpp

Revision:

0:c5fbc6927336

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Sep 30 21:00:04 2012 +0000
@@ -0,0 +1,140 @@
+#include "mbed.h"
+#include "UM6LT.h"
+
+Serial pc(USBTX, USBRX);
+UM6LT imu(p28, p27);
+
+int stdDelayMs = 2;
+
+int main() {
+    
+    pc.baud(115200);
+    imu.baud(115200);
+
+    int broadcastRate = 22;
+    int baudrate = 115200;
+    
+    int wantCov = 0;
+    int wantEulerAngles = 0;
+    int wantQuat = 0;
+    int wantProcMag = 0;
+    int wantProcAccel = 0;
+    int wantProcGyro = 0;
+    int wantRawMag = 0;
+    int wantRawAccel = 0;
+    int wantRawGyro = 0;
+        
+    int dataToTransmit [9] = {wantCov, wantEulerAngles, wantQuat, wantProcMag, wantProcAccel, wantProcGyro, wantRawMag, wantRawAccel, wantRawGyro};
+    int broadcastEnabled = 0;
+    
+    imu.setCommParams(broadcastRate, baudrate, dataToTransmit, broadcastEnabled);
+    
+    int wantPPS = 0;
+    int wantQuatUpdate = 1;
+    int wantCal = 1;
+    int wantAccelUpdate = 1;
+    int wantMagUpdate = 1;
+    
+    imu.setConfigParams(wantPPS, wantQuatUpdate, wantCal, wantAccelUpdate, wantMagUpdate);
+    
+    if(imu.getStatus()){
+        
+        int roll = 0;
+        int pitch = 0;
+        int yaw = 0;
+        int rollRate = 0;
+        int pitchRate = 0;
+        int yawRate = 0;
+        int accelX = 0 ;
+        int accelY = 0 ;
+        int accelZ = 0 ;
+        int magX = 0;
+        int magY = 0;
+        int magZ = 0;
+        int gyroBiasX = 0;
+        int gyroBiasY = 0;
+        int gyroBiasZ = 0;
+        int a = 0;
+        int b = 0;
+        int c = 0;
+        int d = 0;
+        
+        int byteBuffer;
+        
+        for(int i=0; i<5; i++){
+            if(imu.getAngles(roll, pitch, yaw)){
+                wait_ms(stdDelayMs);
+                printf("roll: %d   pitch: %d   yaw: %d\r\n", roll, pitch, yaw);
+            }
+        }
+        
+        printf("\r\n\r\nAngles should be random but consistent\r\n\r\n");
+        
+        if(imu.zeroGyros(gyroBiasX, gyroBiasY,gyroBiasZ)){
+            printf("Gyro Bias X: %d  Gyro Bias Y: %d  Gyro Bias Z: %d\r\n", gyroBiasX, gyroBiasY, gyroBiasZ); 
+        }
+        
+        if(imu.autoSetAccelRef() && imu.autoSetMagRef() && imu.resetEKF()){
+        
+            printf("Press (1) for Euler Angles.\r\n");
+            printf("Press (2) for Accelerations.\r\n");
+            printf("Press (3) for Magnetic field.\r\n");
+            printf("Press (4) for Angle rates.\r\n");
+            printf("Press (5) for Quaternion.\r\n");
+            printf("Press (0) to stop.\r\n");
+            
+            while(!pc.readable()){
+                wait_ms(stdDelayMs);
+            }            
+            byteBuffer = pc.getc();
+        
+            while(1){
+            
+                if(pc.readable()){                    
+                    byteBuffer = pc.getc();
+                }
+                
+                switch(byteBuffer){
+                    case '0':
+                        wait_ms(stdDelayMs);
+                        break;
+                    case '1':
+                        if(imu.getAngles(roll, pitch, yaw)){
+                            wait_ms(stdDelayMs);
+                            printf("roll: %d   pitch: %d   yaw: %d\r\n", roll, pitch, yaw);
+                        }
+                        break;
+                    case '2':
+                        if(imu.getAccel(accelX, accelY, accelZ)){
+                            wait_ms(stdDelayMs);
+                            printf("accelX: %d   accelY: %d   accelZ: %d\r\n", accelX, accelY, accelZ);
+                        }
+                        break;
+                    case '3':
+                        if(imu.getMag(magX, magY, magZ)){
+                            wait_ms(stdDelayMs);
+                            printf("magX: %d   magY: %d   magZ: %d\r\n", magX, magY, magZ);
+                        }
+                        break;
+                    case '4':
+                        if(imu.getAngleRates(rollRate, pitchRate, yawRate)){
+                            wait_ms(stdDelayMs);
+                            printf("rollRate: %d   pitchRate: %d   yawRate: %d\r\n", rollRate, pitchRate, yawRate);
+                        }
+                        break;
+                    case '5':
+                        if(imu.getQuaternion(a, b, c, d)){
+                            wait_ms(stdDelayMs);
+                            printf("a: %d  b: %d  c: %d  d: %d\r\n", a, b, c, d);
+                        }
+                        break;
+                    default:
+                        printf("Wrong command. Enter '1', '2', '3', '4', '5' or '6'.\r\n");
+                        byteBuffer = '0';
+                        break;
+                }
+            }
+            
+        }    
+    }    
+}