Michael Ernst Peter / Mbed OS PM2_Example_Line_Follower

Example project for the Line Follower robot.

Dependencies:   PM2_Libary Eigen

main.cpp@4:67506e285ad0, 2021-04-01 (annotated)

Committer:
pmic
Date:
Thu Apr 01 15:00:55 2021 +0000
Revision:
4:67506e285ad0
Parent:
3:d22942631cd7
Child:
6:e1fa1a2d7483
Included analog sensor readin before switching to mbed studio.

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 2:7c03fefb77ef 53 int servo_desval_0 = 0;
pmic 3:d22942631cd7 54 int servo_desval_1 = 0;
pmic 2:7c03fefb77ef 55
pmic 4:67506e285ad0 56 /* input your stuff here */
pmic 4:67506e285ad0 57 AnalogIn analogIn(PC_2);
pmic 4:67506e285ad0 58 float dist = 0.0f;
pmic 4:67506e285ad0 59
pmic 2:7c03fefb77ef 60 /*
pmic 1:93d997d6b232 61 printf("Test writing... ");
pmic 1:93d997d6b232 62 FILE* fp = fopen("/fs/data.csv", "w");
pmic 1:93d997d6b232 63 fprintf(fp, "test %.5f\r\n",1.23);
pmic 1:93d997d6b232 64 fclose(fp);
pmic 1:93d997d6b232 65 printf("done\r\n");
pmic 1:93d997d6b232 66
pmic 1:93d997d6b232 67 printf("Test reading... ");
pmic 1:93d997d6b232 68 // read from SD card
pmic 1:93d997d6b232 69 fp = fopen("/fs/data.csv", "r");
pmic 1:93d997d6b232 70 if (fp != NULL) {
pmic 1:93d997d6b232 71 char c = fgetc(fp);
pmic 1:93d997d6b232 72 if (c == 't')
pmic 1:93d997d6b232 73 printf("done\r\n");
pmic 1:93d997d6b232 74 else
pmic 1:93d997d6b232 75 printf("incorrect char (%c)!\n", c);
pmic 1:93d997d6b232 76 fclose(fp);
pmic 1:93d997d6b232 77 } else {
pmic 1:93d997d6b232 78 printf("Reading failed!\n");
pmic 1:93d997d6b232 79 }
pmic 2:7c03fefb77ef 80 */
pmic 1:93d997d6b232 81
pmic 1:93d997d6b232 82 // enable driver DC motors
pmic 3:d22942631cd7 83 enable_dc_motors = 1;
pmic 1:93d997d6b232 84
pmic 1:93d997d6b232 85 while (true) {
pmic 1:93d997d6b232 86
pmic 1:93d997d6b232 87 if(!user_button) {
pmic 3:d22942631cd7 88 // LED off, set speedController speed, pwm2, position servo
pmic 1:93d997d6b232 89 myled = 0;
pmic 3:d22942631cd7 90
pmic 3:d22942631cd7 91 speedController_0.setDesiredSpeed( 60.0f);
pmic 3:d22942631cd7 92 speedController_1.setDesiredSpeed(-120.0f);
pmic 3:d22942631cd7 93 pwm_motor_2.write(0.7f);
pmic 1:93d997d6b232 94
pmic 3:d22942631cd7 95 servo_0.SetPosition(servo_desval_0);
pmic 3:d22942631cd7 96 servo_1.SetPosition(servo_desval_1);
pmic 3:d22942631cd7 97 if(servo_desval_0 < 10000) servo_desval_0 += 100;
pmic 3:d22942631cd7 98 if(servo_desval_1 < 10000) servo_desval_1 += 100;
pmic 4:67506e285ad0 99
pmic 4:67506e285ad0 100 dist = analogIn.read()*3.3f;
pmic 1:93d997d6b232 101
pmic 1:93d997d6b232 102 } else {
pmic 3:d22942631cd7 103 // LED on, reset speedController speed, pwm2, position servo
pmic 1:93d997d6b232 104 myled = 1;
pmic 3:d22942631cd7 105
pmic 3:d22942631cd7 106 speedController_0.setDesiredSpeed(0.0f);
pmic 3:d22942631cd7 107 speedController_1.setDesiredSpeed(0.0f);
pmic 3:d22942631cd7 108 pwm_motor_2.write(0.5f);
pmic 1:93d997d6b232 109
pmic 3:d22942631cd7 110 servo_desval_0 = 0.0f;
pmic 3:d22942631cd7 111 servo_desval_1 = 0.0f;
pmic 3:d22942631cd7 112 servo_0.SetPosition(servo_desval_0);
pmic 3:d22942631cd7 113 servo_1.SetPosition(servo_desval_1);
pmic 4:67506e285ad0 114
pmic 4:67506e285ad0 115 dist = analogIn.read()*3.3f;
pmic 1:93d997d6b232 116
pmic 1:93d997d6b232 117 }
pmic 1:93d997d6b232 118
pmic 3:d22942631cd7 119 // printf("speedLeft: %f, speedRight: %f\r\n",controller.getSpeedLeft(), controller.getSpeedRight());
pmic 3:d22942631cd7 120 // printf("counter1 = %d counter2 = %d counter3 = %d\r\n", encoderCounter_0.read(), encoderCounter_1.read(), encoderCounter_2.read());
pmic 4:67506e285ad0 121 printf("speedController_0 = %d speedController_1 = %d encoderCounter_2 = %d measured value in mV: %d\r\n", static_cast<int>(speedController_0.getSpeed()*1000.0f),
pmic 4:67506e285ad0 122 static_cast<int>(speedController_1.getSpeed()*1000.0f),
pmic 4:67506e285ad0 123 encoderCounter_2.read(),
pmic 4:67506e285ad0 124 (static_cast<int>(dist * 1e3)));
pmic 1:93d997d6b232 125
pmic 2:7c03fefb77ef 126 thread_sleep_for(500);
pmic 1:93d997d6b232 127 }
pmic 1:93d997d6b232 128 }