steve imboden / Mbed OS PM2_Example_PES_board

steve

Dependencies:   PM2_Libary

main.cpp@2:7c03fefb77ef, 2021-04-01 (annotated)

Committer:
pmic
Date:
Thu Apr 01 11:18:01 2021 +0000
Revision:
2:7c03fefb77ef
Parent:
1:93d997d6b232
Child:
3:d22942631cd7
First tests with controller and servo successful.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 1:93d997d6b232 1 #include "mbed.h"
pmic 1:93d997d6b232 2 #include "platform/mbed_thread.h"
pmic 1:93d997d6b232 3 #include "SDBlockDevice.h"
pmic 1:93d997d6b232 4 #include "FATFileSystem.h"
pmic 1:93d997d6b232 5 #include "EncoderCounter.h"
pmic 1:93d997d6b232 6 #include "Servo.h"
pmic 1:93d997d6b232 7 #include "Controller.h"
pmic 1:93d997d6b232 8
pmic 1:93d997d6b232 9
pmic 1:93d997d6b232 10 int main()
pmic 1:93d997d6b232 11 {
pmic 1:93d997d6b232 12
pmic 1:93d997d6b232 13 DigitalIn user_button(USER_BUTTON);
pmic 1:93d997d6b232 14
pmic 1:93d997d6b232 15 // initialise PWM
pmic 1:93d997d6b232 16 PwmOut pwm_motor1(PB_13);
pmic 1:93d997d6b232 17 PwmOut pwm_motor2(PA_9);
pmic 1:93d997d6b232 18 PwmOut pwm_motor3(PA_10);
pmic 1:93d997d6b232 19
pmic 1:93d997d6b232 20 // crete Encoder read objects
pmic 1:93d997d6b232 21 EncoderCounter counter1(PA_6, PC_7); // counter(pin A, pin B)
pmic 1:93d997d6b232 22 EncoderCounter counter2(PB_6, PB_7);
pmic 1:93d997d6b232 23 EncoderCounter counter3(PA_0, PA_1);
pmic 1:93d997d6b232 24
pmic 1:93d997d6b232 25 // create controller
pmic 1:93d997d6b232 26 Controller controller(pwm_motor1, pwm_motor2, counter1, counter2);
pmic 1:93d997d6b232 27
pmic 1:93d997d6b232 28 DigitalOut enable(PB_15);
pmic 1:93d997d6b232 29
pmic 1:93d997d6b232 30 // create servo objects
pmic 1:93d997d6b232 31 Servo S0(PB_2);
pmic 1:93d997d6b232 32 Servo S1(PC_8);
pmic 1:93d997d6b232 33 Servo S2(PC_6);
pmic 1:93d997d6b232 34
pmic 1:93d997d6b232 35 SDBlockDevice sd(PC_12, PC_11, PC_10, PD_2);
pmic 1:93d997d6b232 36 printf("BlockDevice created\r\n");
pmic 1:93d997d6b232 37 FATFileSystem fs("fs", &sd);
pmic 1:93d997d6b232 38
pmic 1:93d997d6b232 39 // Initialise the digital pin LED1 as an output
pmic 1:93d997d6b232 40 DigitalOut myled(LED1);
pmic 1:93d997d6b232 41
pmic 1:93d997d6b232 42
pmic 2:7c03fefb77ef 43 /**/
pmic 1:93d997d6b232 44 // initialise PWM
pmic 1:93d997d6b232 45 pwm_motor1.period(0.00005f);// 0.05ms 20KHz
pmic 1:93d997d6b232 46 pwm_motor1.write(0.5f);
pmic 1:93d997d6b232 47 pwm_motor2.period(0.00005f);// 0.05ms 20KHz
pmic 2:7c03fefb77ef 48 pwm_motor2.write(0.5f);
pmic 1:93d997d6b232 49 pwm_motor3.period(0.00005f);// 0.05ms 20KHz
pmic 1:93d997d6b232 50 pwm_motor3.write(0.5f);
pmic 1:93d997d6b232 51
pmic 1:93d997d6b232 52 // initialise and test Servo
pmic 2:7c03fefb77ef 53 S0.Enable(1000,20000); // 1 ms / 20 ms
pmic 1:93d997d6b232 54 S1.Enable(1000,20000);
pmic 1:93d997d6b232 55 S2.Enable(1000,20000);
pmic 1:93d997d6b232 56
pmic 2:7c03fefb77ef 57 int servo_desval_0 = 0;
pmic 2:7c03fefb77ef 58
pmic 2:7c03fefb77ef 59 /*
pmic 1:93d997d6b232 60 printf("Test writing... ");
pmic 1:93d997d6b232 61 FILE* fp = fopen("/fs/data.csv", "w");
pmic 1:93d997d6b232 62 fprintf(fp, "test %.5f\r\n",1.23);
pmic 1:93d997d6b232 63 fclose(fp);
pmic 1:93d997d6b232 64 printf("done\r\n");
pmic 1:93d997d6b232 65
pmic 1:93d997d6b232 66 printf("Test reading... ");
pmic 1:93d997d6b232 67 // read from SD card
pmic 1:93d997d6b232 68 fp = fopen("/fs/data.csv", "r");
pmic 1:93d997d6b232 69 if (fp != NULL) {
pmic 1:93d997d6b232 70 char c = fgetc(fp);
pmic 1:93d997d6b232 71 if (c == 't')
pmic 1:93d997d6b232 72 printf("done\r\n");
pmic 1:93d997d6b232 73 else
pmic 1:93d997d6b232 74 printf("incorrect char (%c)!\n", c);
pmic 1:93d997d6b232 75 fclose(fp);
pmic 1:93d997d6b232 76 } else {
pmic 1:93d997d6b232 77 printf("Reading failed!\n");
pmic 1:93d997d6b232 78 }
pmic 2:7c03fefb77ef 79 */
pmic 1:93d997d6b232 80
pmic 1:93d997d6b232 81 // enable driver DC motors
pmic 1:93d997d6b232 82 enable = 1;
pmic 1:93d997d6b232 83
pmic 1:93d997d6b232 84 while (true) {
pmic 1:93d997d6b232 85
pmic 1:93d997d6b232 86 if(!user_button) {
pmic 1:93d997d6b232 87 // LED off, set controller speed, pwm2, position servo
pmic 1:93d997d6b232 88 myled = 0;
pmic 1:93d997d6b232 89 controller.setDesiredSpeedLeft(50.0f);
pmic 2:7c03fefb77ef 90 // controller.setDesiredSpeedRight(50.0f);
pmic 2:7c03fefb77ef 91 // pwm_motor1.write(0.7f);
pmic 1:93d997d6b232 92 pwm_motor3.write(0.7f);
pmic 1:93d997d6b232 93
pmic 2:7c03fefb77ef 94
pmic 2:7c03fefb77ef 95 S0.SetPosition(servo_desval_0);
pmic 1:93d997d6b232 96 S1.SetPosition(1000);
pmic 1:93d997d6b232 97 S2.SetPosition(1000);
pmic 2:7c03fefb77ef 98 if(servo_desval_0 < 2000) servo_desval_0 += 100;
pmic 1:93d997d6b232 99
pmic 1:93d997d6b232 100 } else {
pmic 1:93d997d6b232 101 // LED on, reset controller speed, pwm2, position servo
pmic 1:93d997d6b232 102 myled = 1;
pmic 1:93d997d6b232 103 controller.setDesiredSpeedLeft(0.0f);
pmic 2:7c03fefb77ef 104 // controller.setDesiredSpeedRight(0.0f);
pmic 2:7c03fefb77ef 105 // pwm_motor1.write(0.5f);
pmic 1:93d997d6b232 106 pwm_motor3.write(0.5f);
pmic 1:93d997d6b232 107
pmic 2:7c03fefb77ef 108 servo_desval_0 = 0;
pmic 2:7c03fefb77ef 109 S0.SetPosition(servo_desval_0);
pmic 1:93d997d6b232 110 S1.SetPosition(1500);
pmic 1:93d997d6b232 111 S2.SetPosition(1500);
pmic 1:93d997d6b232 112
pmic 1:93d997d6b232 113 }
pmic 1:93d997d6b232 114
pmic 1:93d997d6b232 115
pmic 2:7c03fefb77ef 116 //printf("speedLeft: %f, speedRight: %f\r\n",controller.getSpeedLeft(), controller.getSpeedRight());
pmic 2:7c03fefb77ef 117 printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read());
pmic 1:93d997d6b232 118
pmic 2:7c03fefb77ef 119 thread_sleep_for(500);
pmic 1:93d997d6b232 120 }
pmic 1:93d997d6b232 121 }