Diff: main.cpp

Revision:

0:02dd72d1d465

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 24 11:45:18 2018 +0000
@@ -0,0 +1,169 @@
+#include "mbed.h"
+#include "EncoderCounter.h"
+#include "IMU.h"
+#include "Controller.h"
+
+DigitalOut myled(LED1);
+Serial pc1(USBTX, USBRX); // tx, rx
+
+DigitalOut led1(PB_13);
+DigitalOut led2(PC_8);
+DigitalOut led3(PC_9);
+DigitalOut led4(PC_0);
+DigitalOut led5(PC_1);
+DigitalOut led6(PH_1);
+DigitalOut led7(PC_2);
+DigitalOut led8(PC_3);
+DigitalOut led9(PB_0);
+
+// create motor control objects
+DigitalOut enable1(PB_1);
+DigitalOut enable2(PB_2);
+DigitalOut enable3(PB_3);
+
+PwmOut pwm1(PA_8);
+PwmOut pwm2(PA_9);
+PwmOut pwm3(PA_10);
+
+// crete Encoder read objects
+EncoderCounter counter1(PA_6, PC_7);
+EncoderCounter counter2(PB_6, PB_7);
+EncoderCounter counter3(PA_1, PA_0);
+
+// create IMU comunication objects
+SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
+DigitalOut csAG(PA_15);
+DigitalOut csM(PD_2);
+
+// escon I/O
+
+
+//AnalogIn M1_AOUT1(PB_4);
+//AnalogIn M1_AOUT2(PB_5);
+
+//AnalogIn M2_AOUT1(PB_14);
+//AnalogIn M2_AOUT2(PB_15);
+
+AnalogIn M3_AOUT1(PA_7);
+//AnalogIn M3_AOUT2(PA_11);
+
+
+Thread thread;
+
+int main()
+{
+    //IMU
+    IMU imu(spi, csAG, csM);
+
+    pwm1.period_us(200);
+    enable1 = 1;
+    //pwm1.write(0.6f);
+
+    pwm2.period_us(200);
+    enable2 = 1;
+    //pwm2.write(0.6f);
+
+    pwm3.period_us(200);
+    enable3 = 1;
+    //pwm3.write(0.6f);
+    
+    //controller
+    Controller controller(pwm1,pwm2,pwm3,counter1,counter2,counter3,imu);
+    
+    pc1.baud(100000);
+    
+    led1 = 0;
+    led2 = 0;
+    led3 = 0;
+    led4 = 0;
+
+    int i = 1;
+    
+    float gamma_z = 0;
+    float gz_vor = 0;
+    
+    int t = 0;
+    
+    while(1) {
+        
+        led4 = 0;
+        led9 = 0;
+        led5 = 0;
+        led6 = 0;
+        led7 = 0;
+        led8 = 0;
+
+        switch(i) {
+            case 1:
+                led4 = 1;
+                break;
+
+            case 2:
+                led9 = 1;
+                break;
+
+            case 3:
+                led5 = 1;
+                break;
+
+            case 4:
+                led6 = 1;
+                break;
+
+            case 5:
+                led7 = 1;
+                break;
+
+            case 6:
+                led8 = 1;
+                break;
+
+            case 7:
+                led7 = 1;
+                break;
+
+            case 8:
+                led6 = 1;
+                break;
+
+            case 9:
+                led5 = 1;
+                break;
+            case 10:
+                led9 = 1;
+                break;
+
+            default:
+                led1 = 0;
+                led2 = 0;
+                led3 = 0;
+                led4 = 0;
+                break;
+        }
+
+        i++;
+        if(i==11) {
+            i=1;
+        }
+
+        myled = 1; // LED is ON
+        thread.wait(50.0);
+        
+        float gz = imu.readGyroZ();
+        gamma_z = (gz-gz_vor)*0.05f + gamma_z;
+        float f = t*(-0.0011f);
+        
+        //printf("counter1: %d counter2: %d counter3: %d\r\n", counter1.read(),counter2.read(),counter3.read());
+        //printf("%.5f %.5f\r\n",imu.getGammaX(),imu.getGammaY());
+        //printf("%.5f %.5f\r\n",imu.getGammaZ(),imu.getGammaZ());
+        //printf("%.5f %.5f\r\n",imu.readMagnetometerX()*166.0045f -550.0f, imu.readMagnetometerY()*140.6528f + 0.0f);
+        //printf("%.2f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f\r\n",time,imu.readAccelerationX(), imu.readAccelerationY(), imu.readAccelerationZ(),imu.readGyroX(), imu.readGyroY(), imu.readGyroZ(),imu.readMagnetometerX(), imu.readMagnetometerY(), imu.readMagnetometerZ());
+        
+        //printf("%.7f %.7f\r\n",M1_AOUT1.read(),M1_AOUT2.read());
+        
+        //printf("main\r\n");
+        
+        t++;
+
+    }
+}