Michael Ernst Peter / Mbed OS PM2_Example_Line_Follower

Example project for the Line Follower robot.

Dependencies:   PM2_Libary Eigen

main.cpp@3:d22942631cd7, 2021-04-01 (annotated)

Committer:
pmic
Date:
Thu Apr 01 14:31:52 2021 +0000
Revision:
3:d22942631cd7
Parent:
2:7c03fefb77ef
Child:
4:67506e285ad0
Changed Controller to SpeedController to only speedcontrol one motor per instance.

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 3:d22942631cd7 7 #include "SpeedController.h"
pmic 1:93d997d6b232 8
pmic 1:93d997d6b232 9 int main()
pmic 1:93d997d6b232 10 {
pmic 1:93d997d6b232 11
pmic 1:93d997d6b232 12 DigitalIn user_button(USER_BUTTON);
pmic 1:93d997d6b232 13
pmic 1:93d997d6b232 14 // initialise PWM
pmic 3:d22942631cd7 15 PwmOut pwm_motor_0(PB_13);
pmic 3:d22942631cd7 16 PwmOut pwm_motor_1(PA_9);
pmic 3:d22942631cd7 17 PwmOut pwm_motor_2(PA_10);
pmic 1:93d997d6b232 18
pmic 1:93d997d6b232 19 // crete Encoder read objects
pmic 3:d22942631cd7 20 EncoderCounter encoderCounter_0(PA_6, PC_7);
pmic 3:d22942631cd7 21 EncoderCounter encoderCounter_1(PB_6, PB_7);
pmic 3:d22942631cd7 22 EncoderCounter encoderCounter_2(PA_0, PA_1);
pmic 1:93d997d6b232 23
pmic 1:93d997d6b232 24 // create controller
pmic 3:d22942631cd7 25 SpeedController speedController_0(pwm_motor_0, encoderCounter_0);
pmic 3:d22942631cd7 26 SpeedController speedController_1(pwm_motor_1, encoderCounter_1);
pmic 3:d22942631cd7 27
pmic 3:d22942631cd7 28 DigitalOut enable_dc_motors(PB_15);
pmic 1:93d997d6b232 29
pmic 1:93d997d6b232 30 // create servo objects
pmic 3:d22942631cd7 31 Servo servo_0(PB_2);
pmic 3:d22942631cd7 32 Servo servo_1(PC_8);
pmic 3:d22942631cd7 33 Servo servo_2(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 // initialise PWM
pmic 3:d22942631cd7 43 pwm_motor_0.period(0.00005f);// 0.05ms 20KHz
pmic 3:d22942631cd7 44 pwm_motor_0.write(0.5f);
pmic 3:d22942631cd7 45 pwm_motor_1.period(0.00005f);// 0.05ms 20KHz
pmic 3:d22942631cd7 46 pwm_motor_1.write(0.5f);
pmic 3:d22942631cd7 47 pwm_motor_2.period(0.00005f);// 0.05ms 20KHz
pmic 3:d22942631cd7 48 pwm_motor_2.write(0.5f);
pmic 1:93d997d6b232 49
pmic 1:93d997d6b232 50 // initialise and test Servo
pmic 3:d22942631cd7 51 servo_0.Enable(1000, 20000); // 1 ms / 20 ms
pmic 3:d22942631cd7 52 servo_1.Enable(1000, 20000);
pmic 3:d22942631cd7 53 servo_2.Enable(1000, 20000);
pmic 2:7c03fefb77ef 54 int servo_desval_0 = 0;
pmic 3:d22942631cd7 55 int servo_desval_1 = 0;
pmic 3:d22942631cd7 56 int servo_desval_2 = 0;
pmic 2:7c03fefb77ef 57
pmic 2:7c03fefb77ef 58 /*
pmic 1:93d997d6b232 59 printf("Test writing... ");
pmic 1:93d997d6b232 60 FILE* fp = fopen("/fs/data.csv", "w");
pmic 1:93d997d6b232 61 fprintf(fp, "test %.5f\r\n",1.23);
pmic 1:93d997d6b232 62 fclose(fp);
pmic 1:93d997d6b232 63 printf("done\r\n");
pmic 1:93d997d6b232 64
pmic 1:93d997d6b232 65 printf("Test reading... ");
pmic 1:93d997d6b232 66 // read from SD card
pmic 1:93d997d6b232 67 fp = fopen("/fs/data.csv", "r");
pmic 1:93d997d6b232 68 if (fp != NULL) {
pmic 1:93d997d6b232 69 char c = fgetc(fp);
pmic 1:93d997d6b232 70 if (c == 't')
pmic 1:93d997d6b232 71 printf("done\r\n");
pmic 1:93d997d6b232 72 else
pmic 1:93d997d6b232 73 printf("incorrect char (%c)!\n", c);
pmic 1:93d997d6b232 74 fclose(fp);
pmic 1:93d997d6b232 75 } else {
pmic 1:93d997d6b232 76 printf("Reading failed!\n");
pmic 1:93d997d6b232 77 }
pmic 2:7c03fefb77ef 78 */
pmic 1:93d997d6b232 79
pmic 1:93d997d6b232 80 // enable driver DC motors
pmic 3:d22942631cd7 81 enable_dc_motors = 1;
pmic 1:93d997d6b232 82
pmic 1:93d997d6b232 83 while (true) {
pmic 1:93d997d6b232 84
pmic 1:93d997d6b232 85 if(!user_button) {
pmic 3:d22942631cd7 86 // LED off, set speedController speed, pwm2, position servo
pmic 1:93d997d6b232 87 myled = 0;
pmic 3:d22942631cd7 88
pmic 3:d22942631cd7 89 speedController_0.setDesiredSpeed( 60.0f);
pmic 3:d22942631cd7 90 speedController_1.setDesiredSpeed(-120.0f);
pmic 3:d22942631cd7 91 pwm_motor_2.write(0.7f);
pmic 1:93d997d6b232 92
pmic 3:d22942631cd7 93 servo_0.SetPosition(servo_desval_0);
pmic 3:d22942631cd7 94 servo_1.SetPosition(servo_desval_1);
pmic 3:d22942631cd7 95 servo_2.SetPosition(servo_desval_2);
pmic 3:d22942631cd7 96 if(servo_desval_0 < 10000) servo_desval_0 += 100;
pmic 3:d22942631cd7 97 if(servo_desval_1 < 10000) servo_desval_1 += 100;
pmic 3:d22942631cd7 98 if(servo_desval_2 < 10000) servo_desval_2 += 100;
pmic 1:93d997d6b232 99
pmic 1:93d997d6b232 100 } else {
pmic 3:d22942631cd7 101 // LED on, reset speedController speed, pwm2, position servo
pmic 1:93d997d6b232 102 myled = 1;
pmic 3:d22942631cd7 103
pmic 3:d22942631cd7 104 speedController_0.setDesiredSpeed(0.0f);
pmic 3:d22942631cd7 105 speedController_1.setDesiredSpeed(0.0f);
pmic 3:d22942631cd7 106 pwm_motor_2.write(0.5f);
pmic 1:93d997d6b232 107
pmic 3:d22942631cd7 108 servo_desval_0 = 0.0f;
pmic 3:d22942631cd7 109 servo_desval_1 = 0.0f;
pmic 3:d22942631cd7 110 servo_desval_2 = 0.0f;
pmic 3:d22942631cd7 111 servo_0.SetPosition(servo_desval_0);
pmic 3:d22942631cd7 112 servo_1.SetPosition(servo_desval_1);
pmic 3:d22942631cd7 113 servo_2.SetPosition(servo_desval_2);
pmic 1:93d997d6b232 114
pmic 1:93d997d6b232 115 }
pmic 1:93d997d6b232 116
pmic 3:d22942631cd7 117 // printf("speedLeft: %f, speedRight: %f\r\n",controller.getSpeedLeft(), controller.getSpeedRight());
pmic 3:d22942631cd7 118 // printf("counter1 = %d counter2 = %d counter3 = %d\r\n", encoderCounter_0.read(), encoderCounter_1.read(), encoderCounter_2.read());
pmic 3:d22942631cd7 119 printf("speedController_0 = %d speedController_1 = %d encoderCounter_2 = %d\r\n", static_cast<int>(speedController_0.getSpeed()*1000.0f),
pmic 3:d22942631cd7 120 static_cast<int>(speedController_1.getSpeed()*1000.0f),
pmic 3:d22942631cd7 121 encoderCounter_2.read());
pmic 1:93d997d6b232 122
pmic 2:7c03fefb77ef 123 thread_sleep_for(500);
pmic 1:93d997d6b232 124 }
pmic 1:93d997d6b232 125 }