Nicolas Borla
/
Example_for_Joel
IMU measurement + Speed controller
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?
User | Revision | Line number | New 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 |