Michael Ernst Peter / Mbed OS PM2_Example_Line_Follower

Example project for the Line Follower robot.

Dependencies:   PM2_Libary Eigen

main.cpp@1:93d997d6b232, 2021-03-30 (annotated)

Committer:
pmic
Date:
Tue Mar 30 14:20:41 2021 +0000
Revision:
1:93d997d6b232
Parent:
0:f44522e28559
Child:
2:7c03fefb77ef
First commit.

Who changed what in which revision?

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