Nicolas Borla
/
Example_for_Joel
IMU measurement + Speed controller
main.cpp
- Committer:
- boro
- Date:
- 2019-05-30
- Revision:
- 0:5a93e4916fb1
- Child:
- 1:17fdd812cb8d
File content as of revision 0:5a93e4916fb1:
#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 } }