Nicolas Borla / Mbed 2 deprecated Example_for_Joel

IMU measurement + Speed controller

Dependencies:   mbed

main.cpp@1:17fdd812cb8d, 2019-05-30 (annotated)

Committer:
boro
Date:
Thu May 30 13:21:44 2019 +0000
Revision:
1:17fdd812cb8d
Parent:
0:5a93e4916fb1 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
boro 0:5a93e4916fb1 1 #include "mbed.h"
boro 0:5a93e4916fb1 2 #include "EncoderCounter.h"
boro 0:5a93e4916fb1 3 #include "Controller.h"
boro 0:5a93e4916fb1 4 #include "IMU.h"
boro 0:5a93e4916fb1 5
boro 0:5a93e4916fb1 6 DigitalOut myled(LED1);
boro 0:5a93e4916fb1 7 DigitalOut enable(PB_15);
boro 0:5a93e4916fb1 8
boro 0:5a93e4916fb1 9 // create IMU comunication objects
boro 0:5a93e4916fb1 10 SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
boro 0:5a93e4916fb1 11 DigitalOut csAG(PA_15);
boro 0:5a93e4916fb1 12 DigitalOut csM(PD_2);
boro 0:5a93e4916fb1 13
boro 0:5a93e4916fb1 14 //create IMU object
boro 0:5a93e4916fb1 15 IMU imu(spi, csAG, csM);
boro 0:5a93e4916fb1 16
boro 0:5a93e4916fb1 17 // initialise PWM
boro 0:5a93e4916fb1 18 PwmOut pwm_motor1(PB_13);
boro 0:5a93e4916fb1 19 PwmOut pwm_motor2(PA_9);
boro 0:5a93e4916fb1 20
boro 0:5a93e4916fb1 21 // crete Encoder read objects
boro 0:5a93e4916fb1 22 EncoderCounter counter1(PA_6, PC_7); // counter(pin A, pin B)
boro 0:5a93e4916fb1 23 EncoderCounter counter2(PB_6, PB_7);
boro 0:5a93e4916fb1 24
boro 0:5a93e4916fb1 25 // create controller object
boro 0:5a93e4916fb1 26 Controller controller(pwm_motor1, pwm_motor2, counter1, counter2);
boro 0:5a93e4916fb1 27
boro 0:5a93e4916fb1 28 DigitalIn user_button(USER_BUTTON);
boro 0:5a93e4916fb1 29
boro 0:5a93e4916fb1 30 int main()
boro 0:5a93e4916fb1 31 {
boro 0:5a93e4916fb1 32 // initialise controller for 2 DC-Motor
boro 0:5a93e4916fb1 33 controller.setDesiredSpeedLeft(0.0f); // speed in [rpm]
boro 0:5a93e4916fb1 34 controller.setDesiredSpeedRight(0.0f);
boro 0:5a93e4916fb1 35 enable = 1;
boro 0:5a93e4916fb1 36
boro 0:5a93e4916fb1 37 // variables for measurement in one axis
boro 1:17fdd812cb8d 38 int nData = 5000;
boro 1:17fdd812cb8d 39 int nSave = 1;
boro 0:5a93e4916fb1 40 int delayMeasure = 3000;
boro 0:5a93e4916fb1 41 int * T = new int[nData]();
boro 0:5a93e4916fb1 42 float * Mes1 = new float[nData]();
boro 0:5a93e4916fb1 43 float * Mes2 = new float[nData]();
boro 0:5a93e4916fb1 44 int mesInd;
boro 0:5a93e4916fb1 45
boro 0:5a93e4916fb1 46 int t = 0;
boro 0:5a93e4916fb1 47 bool sendMeasure = false;
boro 0:5a93e4916fb1 48
boro 0:5a93e4916fb1 49 bool stable = true;
boro 0:5a93e4916fb1 50 bool previousStable = true;
boro 0:5a93e4916fb1 51
boro 0:5a93e4916fb1 52 printf("initialization succesfull\n\r");
boro 0:5a93e4916fb1 53
boro 0:5a93e4916fb1 54 while (1) {
boro 0:5a93e4916fb1 55
boro 0:5a93e4916fb1 56 if(!user_button) {
boro 0:5a93e4916fb1 57 // LED off, set target speed at 100rpm
boro 0:5a93e4916fb1 58 myled = 0;
boro 0:5a93e4916fb1 59 controller.setDesiredSpeedLeft(60.0f); // set speed 100rpm
boro 0:5a93e4916fb1 60 controller.setDesiredSpeedRight(60.0f);
boro 0:5a93e4916fb1 61 } else {
boro 0:5a93e4916fb1 62 // LED on, set target speed at 0rpm
boro 0:5a93e4916fb1 63 myled = 1;
boro 0:5a93e4916fb1 64 controller.setDesiredSpeedLeft(0.0f); // Drehzahl in [rpm]
boro 0:5a93e4916fb1 65 controller.setDesiredSpeedRight(0.0f);
boro 0:5a93e4916fb1 66 }
boro 0:5a93e4916fb1 67 // display encoder values to console
boro 0:5a93e4916fb1 68 printf("speed left = %.2f rpm, speed right = %.2f rpm\r\n",controller.getSpeedLeft(),controller.getSpeedRight());
boro 0:5a93e4916fb1 69
boro 0:5a93e4916fb1 70
boro 0:5a93e4916fb1 71 float accX = imu.readAccelerationX();
boro 0:5a93e4916fb1 72
boro 0:5a93e4916fb1 73 // save data in object measure-variables
boro 1:17fdd812cb8d 74 if (t % nSave == 0 && stable) {
boro 0:5a93e4916fb1 75 *(T+mesInd) = t;
boro 0:5a93e4916fb1 76 *(Mes1+mesInd) = accX;
boro 0:5a93e4916fb1 77 *(Mes2+mesInd) = accX;
boro 0:5a93e4916fb1 78 mesInd ++;
boro 0:5a93e4916fb1 79 }
boro 0:5a93e4916fb1 80 // reset index if array are full
boro 0:5a93e4916fb1 81 if(mesInd == nData){mesInd = 0;}
boro 0:5a93e4916fb1 82
boro 1:17fdd812cb8d 83 // stability condition
boro 1:17fdd812cb8d 84 //if(...){stable = false;}
boro 0:5a93e4916fb1 85
boro 0:5a93e4916fb1 86 // send measure to console
boro 0:5a93e4916fb1 87 if(sendMeasure) {
boro 0:5a93e4916fb1 88 printf("SendMeasure\r\n\n");
boro 0:5a93e4916fb1 89 for(int j=mesInd; j<(nData-mesInd); j++) {
boro 0:5a93e4916fb1 90 printf("%d %.7f %.7f\r\n",*(T+j)-*(T+mesInd),*(Mes1+j),*(Mes2+j));
boro 0:5a93e4916fb1 91 }
boro 0:5a93e4916fb1 92 for(int j=0; j<mesInd; j++) {
boro 0:5a93e4916fb1 93 printf("%d %.7f %.7f\r\n",*(T+j)-*(T+mesInd),*(Mes1+j),*(Mes2+j));
boro 0:5a93e4916fb1 94 }
boro 0:5a93e4916fb1 95 printf("endData\r\n\n");
boro 0:5a93e4916fb1 96 sendMeasure = false;
boro 0:5a93e4916fb1 97 }
boro 0:5a93e4916fb1 98
boro 0:5a93e4916fb1 99 t++;
boro 0:5a93e4916fb1 100
boro 0:5a93e4916fb1 101 wait(0.01f); // Takt 0.01s, 100Hz
boro 0:5a93e4916fb1 102 }
boro 0:5a93e4916fb1 103 }
boro 0:5a93e4916fb1 104
boro 0:5a93e4916fb1 105
boro 0:5a93e4916fb1 106