Diff: main.cpp

Revision:

0:5a93e4916fb1

Child:

1:17fdd812cb8d

diff -r 000000000000 -r 5a93e4916fb1 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu May 30 12:41:57 2019 +0000
@@ -0,0 +1,106 @@
+#include "mbed.h"
+#include "EncoderCounter.h"
+#include "Controller.h"
+#include "IMU.h"
+
+DigitalOut myled(LED1);
+DigitalOut enable(PB_15);
+
+// create IMU comunication objects
+SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
+DigitalOut csAG(PA_15);
+DigitalOut csM(PD_2);
+
+//create IMU object
+IMU imu(spi, csAG, csM);
+
+// initialise PWM
+PwmOut pwm_motor1(PB_13);
+PwmOut pwm_motor2(PA_9);
+
+// crete Encoder read objects
+EncoderCounter counter1(PA_6, PC_7); // counter(pin A, pin B)
+EncoderCounter counter2(PB_6, PB_7);
+
+// create controller object
+Controller controller(pwm_motor1, pwm_motor2, counter1, counter2);
+
+DigitalIn user_button(USER_BUTTON);
+
+int main()
+{
+    // initialise controller for 2 DC-Motor
+    controller.setDesiredSpeedLeft(0.0f); // speed in [rpm]
+    controller.setDesiredSpeedRight(0.0f);
+    enable = 1;
+    
+    // variables for measurement in one axis
+    int nData = 1500;
+    int nSave = 10;
+    int delayMeasure = 3000;
+    int   * T = new int[nData]();
+    float * Mes1 = new float[nData]();
+    float * Mes2 = new float[nData]();
+    int mesInd;
+    
+    int t = 0;
+    bool sendMeasure = false;
+    
+    bool stable = true;
+    bool previousStable = true;
+        
+    printf("initialization succesfull\n\r");
+
+    while (1) {
+
+        if(!user_button) {            
+            // LED off, set target speed at 100rpm
+            myled = 0;
+            controller.setDesiredSpeedLeft(60.0f); // set speed 100rpm
+            controller.setDesiredSpeedRight(60.0f);    
+        } else {
+            // LED on, set target speed at 0rpm   
+            myled = 1;
+            controller.setDesiredSpeedLeft(0.0f); // Drehzahl in [rpm]
+            controller.setDesiredSpeedRight(0.0f);
+        }
+        // display encoder values to console
+        printf("speed left = %.2f rpm, speed right = %.2f rpm\r\n",controller.getSpeedLeft(),controller.getSpeedRight());
+
+
+        float accX = imu.readAccelerationX();
+
+        // save data in object measure-variables
+        if (t % nSave == 0) {
+                *(T+mesInd) = t;
+                *(Mes1+mesInd) = accX;
+                *(Mes2+mesInd) = accX; 
+                mesInd ++;
+            }
+        // reset index if array are full
+        if(mesInd == nData){mesInd = 0;}
+
+        if(previousStable&&!stable){sendMeasure == true;}
+        previousStable = stable;
+        
+        // send measure to console
+        if(sendMeasure) {
+            printf("SendMeasure\r\n\n");
+            for(int j=mesInd; j<(nData-mesInd); j++) {
+                printf("%d %.7f %.7f\r\n",*(T+j)-*(T+mesInd),*(Mes1+j),*(Mes2+j));
+            }
+            for(int j=0; j<mesInd; j++) {
+                printf("%d %.7f %.7f\r\n",*(T+j)-*(T+mesInd),*(Mes1+j),*(Mes2+j));
+            }
+            printf("endData\r\n\n");
+            sendMeasure = false;
+        }
+        
+        t++;
+        
+        wait(0.01f); // Takt 0.01s, 100Hz  
+    }
+}
+
+
+