![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
PES2 Firmware for mbed2
Dependencies: Stepper mbed Servo LSM9DS1_Library SDFileSystem
Diff: main.cpp
- Revision:
- 0:41afd907b7bd
diff -r 000000000000 -r 41afd907b7bd main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Mar 12 13:06:52 2021 +0000 @@ -0,0 +1,296 @@ +#include "mbed.h" +#include "LSM9DS1.h" +#include "Servo.h" +#include "EncoderCounter.h" +#include "Stepper.h" +#include "SDFileSystem.h" + +DigitalOut myled(LED1); + +// create object IMU +LSM9DS1 imu(PC_9, PA_8, 0xD6, 0x3C); + +// create servo objects +Servo S0(PB_2); +Servo S1(PC_8); +Servo S2(PC_6); + +DigitalOut enable(PB_15); + +// initialise PWM +PwmOut pwm_motor1(PB_13); +PwmOut pwm_motor2(PA_9); +PwmOut pwm_motor3(PA_10); + +// crete Encoder read objects +EncoderCounter counter1(PA_6, PC_7); // counter(pin A, pin B) +EncoderCounter counter2(PB_6, PB_7); +EncoderCounter counter3(PA_0, PA_1); + +// initialise PWM +Stepper stepperMotor(PB_14,PC_4); // step, dir +Stepper stepperMotor2(PB_12,PA_15); // config 2 + +DigitalIn user_button(USER_BUTTON); + +SDFileSystem sd(PC_12, PC_11, PC_10, PD_2, "sd"); // mosi miso clk cs +AnalogIn ain(PC_2); +DigitalOut enableStepper(PB_1); + +// define write function +void sdWrite(float a){ + printf("start write SD\r\n"); + + FILE *fp = fopen("/sd/mydir/sdtest.txt", "a"); + if(fp == NULL) { + error("Could not open file for write\r\n"); + } + fprintf(fp, "analog measure %f\r\n",a); + fclose(fp); + + printf("write succesfull\r\n\n"); +} + +int main() { + // test comunication + imu.begin(); + if (!imu.begin()) { + printf("Failed to communicate with LSM9DS1.\r\n"); + } + + // initialise and test Servo + S0.Enable(1000,20000); + S1.Enable(1000,20000); + S2.Enable(1000,20000); + + // initialise PWM + pwm_motor1.period(0.0005f);// 0.5ms 2KHz + pwm_motor1.write(0.5f); + pwm_motor2.period(0.0005f);// 0.5ms 2KHz + pwm_motor2.write(0.5f); + pwm_motor3.period(0.0005f);// 0.5ms 2KHz + pwm_motor3.write(0.5f); + + // initialise stepper + stepperMotor.setSpeed(10000); + stepperMotor.rotate(stepperMotor.CW); + stepperMotor.stop(); + stepperMotor.setPositionZero(); + + stepperMotor2.setSpeed(10000); + stepperMotor2.rotate(stepperMotor.CW); + stepperMotor2.stop(); + stepperMotor2.setPositionZero(); + + int i = 0; + + bool buttonNow = false; + bool buttonBefore = false; + bool done = false; + + enableStepper = 1; + + + while(1) { + + + buttonNow = !user_button; + + if(buttonNow && !buttonBefore){i+=1; done = false;} + if(i>7){i=0;} + + switch (i){ + case 1: + // =================== + // IMU + // =================== + if(!done){ + printf("\r\n\r\n======================\r\nIMU test\r\n======================\r\n\r\n"); + for(int i=0; i<5; i++){ + // do a measure + imu.readAccel(); + imu.readMag(); + imu.readGyro(); + + // print values to console + printf("gyro: %.5f %.5f %.5f [rad/s]\r\n", imu.gyroX,imu.gyroY,imu.gyroZ); + printf("accel: %.5f %.5f %.5f [m/s^2]\r\n", imu.accX,imu.accY,imu.accZ); + printf("mag: %.5f %.5f %.5f [Gauss]\r\n\n", imu.magX,imu.magY,imu.magZ); + wait(0.5f); + } + done = true; + break; + } else { + break; + } + + case 2: + // =================== + // Servo + // =================== + if(!done){ + printf("\r\n\r\n======================\r\nServo test\r\n======================\r\n\r\n"); + for(int j=0; j<2000; j+=50){ + printf("position %d\r\n",j); + S0.SetPosition(j); + S1.SetPosition(j); + S2.SetPosition(j); + + wait(0.1f); + } + done = true; + break; + } else { + break; + } + + case 3: + // =================== + // DC - Motor1 + // =================== + if(!done){ + printf("\r\n\r\n======================\r\nDC - Motor1 test\r\n======================\r\n\r\n"); + enable = 1; + for(int j=0; j<20; j+=1){ + pwm_motor1.write(0.7f); + pwm_motor2.write(0.7f); + pwm_motor3.write(0.7f); + // display encoder values to console + printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read()); + wait(0.1f); + } + enable = 0; + done = true; + break; + } else { + break; + } + + case 4: + // =================== + // DC - Motor2 + // =================== + if(!done){ + printf("\r\n\r\n======================\r\nDC - Motor2 test\r\n======================\r\n\r\n"); + enable = 1; + for(int j=0; j<20; j+=1){ + pwm_motor1.write(0.7f); + pwm_motor2.write(0.7f); + pwm_motor3.write(0.7f); + // display encoder values to console + printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read()); + wait(0.1f); + } + enable = 0; + done = true; + break; + } else { + break; + } + + case 5: + // =================== + // DC - Motor3 + // =================== + if(!done){ + printf("\r\n\r\n======================\r\nDC - Motor3 test\r\n======================\r\n\r\n"); + enable = 1; + for(int j=0; j<20; j+=1){ + pwm_motor1.write(0.7f); + pwm_motor2.write(0.7f); + pwm_motor3.write(0.7f); + // display encoder values to console + printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read()); + wait(0.1f); + } + enable = 0; + done = true; + break; + } else { + break; + } + + case 6: + // =================== + // Stepper + // =================== + if(!done){ + printf("\r\n\r\n======================\r\nStepper test\r\n======================\r\n\r\n"); + // test stepper motor + enableStepper = 0; + wait(0.5f); + printf("stepper enabled\r\n"); + stepperMotor.goesTo(20*2048); + stepperMotor2.goesTo(20*2048); + wait(3.0f); + stepperMotor.stop(); + stepperMotor2.stop(); + stepperMotor.setPositionZero(); + stepperMotor2.setPositionZero(); + enableStepper = 1; + printf("stepper done\r\n"); + done = true; + break; + } else { + break; + } + + case 7: + // =================== + // SD + // =================== + if(!done){ + printf("\r\n\r\n======================\r\nSD test\r\n======================\r\n\r\n"); + + // crearte folder + mkdir("/sd/mydir", 0777); + + // create file + printf("start write SD\r\n"); + FILE *fp = fopen("/sd/mydir/sdtest.txt", "w"); + if(fp == NULL) { + error("Could not open file for write\r\n"); + } + fprintf(fp, "analog measure\r\n"); + fprintf(fp, "==============\r\n"); + fclose(fp); + + // write 2 times an analog value + for(int i=0; i<2; i++){ + sdWrite(ain.read()); + wait(2.0f); + } + // read from SD card + printf("Reading from SD card..."); + fp = fopen("/sd/mydir/sdtest.txt", "r"); + if (fp != NULL) { + char c = fgetc(fp); + if (c == 'a') + printf("success!\n"); + else + printf("incorrect char (%c)!\n", c); + fclose(fp); + } else { + printf("failed!\n"); + } + + done = true; + break; + } else { + break; + } + + + default: + printf("\r\n\r\n======================\r\nPause\r\n======================\r\n\r\n"); + break; + } + + + myled = 1; + + wait(0.1); // 0.5s + + myled = 0; + } +}