![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
PES2 Firmware for mbed2
Dependencies: Stepper mbed Servo LSM9DS1_Library SDFileSystem
main.cpp
- Committer:
- boro
- Date:
- 2021-03-12
- Revision:
- 0:41afd907b7bd
File content as of revision 0:41afd907b7bd:
#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; } }