Nicolas Borla
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;
+ }
+}