Nicolas Borla
/
Example_for_Joel
IMU measurement + Speed controller
Diff: main.cpp
- Revision:
- 0:5a93e4916fb1
- Child:
- 1:17fdd812cb8d
--- /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 + } +} + + +