Michael Ernst Peter / Mbed OS PM2_Example_Line_Follower

Example project for the Line Follower robot.

Dependencies:   PM2_Libary Eigen

main.cpp@9:f10b974d01e0, 2021-04-06 (annotated)

Committer:
pmic
Date:
Tue Apr 06 12:19:34 2021 +0000
Revision:
9:f10b974d01e0
Parent:
8:9bb806a7f585
Child:
10:c5d85e35758c
Introduce FastPWM Libary.

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 6:e1fa1a2d7483 3
pmic 6:e1fa1a2d7483 4 /* PM2_Libary */
pmic 1:93d997d6b232 5 #include "EncoderCounter.h"
pmic 1:93d997d6b232 6 #include "Servo.h"
pmic 3:d22942631cd7 7 #include "SpeedController.h"
pmic 9:f10b974d01e0 8 #include "FastPWM.h"
pmic 6:e1fa1a2d7483 9 // #include "FATFileSystem.h"
pmic 6:e1fa1a2d7483 10 // #include "SDBlockDevice.h"
pmic 6:e1fa1a2d7483 11
pmic 6:e1fa1a2d7483 12 using namespace std::chrono;
pmic 6:e1fa1a2d7483 13
pmic 8:9bb806a7f585 14 InterruptIn user_button(USER_BUTTON);
pmic 8:9bb806a7f585 15 DigitalOut led(LED1);
pmic 6:e1fa1a2d7483 16
pmic 6:e1fa1a2d7483 17 bool executeMainTask = false;
pmic 6:e1fa1a2d7483 18 Timer user_button_timer, loop_timer;
pmic 7:c0f5bb355f41 19 int Ts_ms = 50;
pmic 6:e1fa1a2d7483 20
pmic 6:e1fa1a2d7483 21 /* declaration of custom button functions */
pmic 6:e1fa1a2d7483 22 void button_fall();
pmic 6:e1fa1a2d7483 23 void button_rise();
pmic 6:e1fa1a2d7483 24
pmic 6:e1fa1a2d7483 25 /* create analog input object */
pmic 6:e1fa1a2d7483 26 AnalogIn analogIn(PC_2);
pmic 6:e1fa1a2d7483 27 float dist = 0.0f;
pmic 6:e1fa1a2d7483 28
pmic 6:e1fa1a2d7483 29 /* create pwm objects */
pmic 9:f10b974d01e0 30 FastPWM pwmOut_m0(PB_13);
pmic 9:f10b974d01e0 31 FastPWM pwmOut_m1(PA_9);
pmic 9:f10b974d01e0 32 FastPWM pwmOut_m2(PA_10);
pmic 9:f10b974d01e0 33 double Ts_pwm_s = 0.00005; // this needs to be a double value
pmic 6:e1fa1a2d7483 34 /* create enable dc motor digital out object */
pmic 6:e1fa1a2d7483 35 DigitalOut enable_motors(PB_15);
pmic 6:e1fa1a2d7483 36 /* create encoder read objects */
pmic 6:e1fa1a2d7483 37 EncoderCounter encoderCounter_m0(PA_6, PC_7);
pmic 6:e1fa1a2d7483 38 EncoderCounter encoderCounter_m1(PB_6, PB_7);
pmic 6:e1fa1a2d7483 39 EncoderCounter encoderCounter_m2(PA_0, PA_1);
pmic 6:e1fa1a2d7483 40 /* create speed controller objects, only m0 and m1, m2 is used open-loop */
pmic 6:e1fa1a2d7483 41 SpeedController speedController_m0(1562.5f, 15.0f, 0.1f, 12.0f, pwmOut_m0, encoderCounter_m0);
pmic 6:e1fa1a2d7483 42 SpeedController speedController_m1(1562.5f, 15.0f, 0.1f, 12.0f, pwmOut_m1, encoderCounter_m1);
pmic 6:e1fa1a2d7483 43
pmic 6:e1fa1a2d7483 44 /* create servo objects */
pmic 6:e1fa1a2d7483 45 Servo servo_0(PB_2);
pmic 6:e1fa1a2d7483 46 Servo servo_1(PC_8);
pmic 6:e1fa1a2d7483 47 Servo servo_2(PC_6); // not used in this example
pmic 8:9bb806a7f585 48 int servoHolePeriod_mus = 20000;
pmic 8:9bb806a7f585 49 int servoPeriod_mus_0 = 0;
pmic 8:9bb806a7f585 50 int servoPeriod_mus_1 = 0;
pmic 8:9bb806a7f585 51 int counter = 0;
pmic 8:9bb806a7f585 52 int loops_per_second = (int)ceilf(1.0f/(0.001f*(float)Ts_ms));
pmic 6:e1fa1a2d7483 53
pmic 6:e1fa1a2d7483 54 /* create sd object */
pmic 6:e1fa1a2d7483 55 // SDBlockDevice sd(PC_12, PC_11, PC_10, PD_2);
pmic 6:e1fa1a2d7483 56 // FATFileSystem fs("fs", &sd);
pmic 1:93d997d6b232 57
pmic 1:93d997d6b232 58 int main()
pmic 9:f10b974d01e0 59 {
pmic 6:e1fa1a2d7483 60 user_button.fall(&button_fall);
pmic 6:e1fa1a2d7483 61 user_button.rise(&button_rise);
pmic 6:e1fa1a2d7483 62 loop_timer.start();
pmic 6:e1fa1a2d7483 63
pmic 6:e1fa1a2d7483 64 /* initialize pwm */
pmic 9:f10b974d01e0 65 pwmOut_m2.period(Ts_pwm_s);
pmic 6:e1fa1a2d7483 66 /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max */
pmic 9:f10b974d01e0 67 pwmOut_m2.write(0.5);
pmic 6:e1fa1a2d7483 68 /* enable driver DC motors */
pmic 6:e1fa1a2d7483 69 enable_motors = 1;
pmic 6:e1fa1a2d7483 70
pmic 6:e1fa1a2d7483 71 /* initialize servo */
pmic 8:9bb806a7f585 72 servo_0.Enable(servoPeriod_mus_0, servoHolePeriod_mus); // 1 ms / 20 ms
pmic 8:9bb806a7f585 73 servo_1.Enable(servoPeriod_mus_0, servoHolePeriod_mus);
pmic 9:f10b974d01e0 74
pmic 2:7c03fefb77ef 75 /*
pmic 6:e1fa1a2d7483 76 // example code for sd card, not tested from pmic, 02.04.2021
pmic 1:93d997d6b232 77 printf("Test writing... ");
pmic 1:93d997d6b232 78 FILE* fp = fopen("/fs/data.csv", "w");
pmic 1:93d997d6b232 79 fprintf(fp, "test %.5f\r\n",1.23);
pmic 1:93d997d6b232 80 fclose(fp);
pmic 1:93d997d6b232 81 printf("done\r\n");
pmic 6:e1fa1a2d7483 82
pmic 1:93d997d6b232 83 printf("Test reading... ");
pmic 1:93d997d6b232 84 // read from SD card
pmic 1:93d997d6b232 85 fp = fopen("/fs/data.csv", "r");
pmic 1:93d997d6b232 86 if (fp != NULL) {
pmic 1:93d997d6b232 87 char c = fgetc(fp);
pmic 1:93d997d6b232 88 if (c == 't')
pmic 1:93d997d6b232 89 printf("done\r\n");
pmic 1:93d997d6b232 90 else
pmic 1:93d997d6b232 91 printf("incorrect char (%c)!\n", c);
pmic 1:93d997d6b232 92 fclose(fp);
pmic 1:93d997d6b232 93 } else {
pmic 1:93d997d6b232 94 printf("Reading failed!\n");
pmic 1:93d997d6b232 95 }
pmic 2:7c03fefb77ef 96 */
pmic 6:e1fa1a2d7483 97
pmic 1:93d997d6b232 98 while (true) {
pmic 6:e1fa1a2d7483 99
pmic 6:e1fa1a2d7483 100 loop_timer.reset();
pmic 6:e1fa1a2d7483 101
pmic 6:e1fa1a2d7483 102 /* ------------- start hacking ------------- -------------*/
pmic 6:e1fa1a2d7483 103
pmic 6:e1fa1a2d7483 104 if (executeMainTask) {
pmic 6:e1fa1a2d7483 105
pmic 6:e1fa1a2d7483 106 /* read analog input */
pmic 6:e1fa1a2d7483 107 dist = analogIn.read() * 3.3f;
pmic 6:e1fa1a2d7483 108
pmic 9:f10b974d01e0 109 speedController_m0.setDesiredSpeedRPS( 1.0f);
pmic 9:f10b974d01e0 110 speedController_m1.setDesiredSpeedRPS(-0.5f);
pmic 9:f10b974d01e0 111 pwmOut_m2.write(0.75);
pmic 6:e1fa1a2d7483 112
pmic 8:9bb806a7f585 113 servo_0.SetPosition(servoPeriod_mus_0);
pmic 8:9bb806a7f585 114 servo_1.SetPosition(servoPeriod_mus_1);
pmic 8:9bb806a7f585 115 if (servoPeriod_mus_0 <= servoHolePeriod_mus & counter%loops_per_second == 0 & counter != 0) {
pmic 8:9bb806a7f585 116 servoPeriod_mus_0 += 100;
pmic 8:9bb806a7f585 117 }
pmic 8:9bb806a7f585 118 if (servoPeriod_mus_1 <= servoHolePeriod_mus & counter%loops_per_second == 0 & counter != 0) {
pmic 8:9bb806a7f585 119 servoPeriod_mus_1 += 100;
pmic 8:9bb806a7f585 120 }
pmic 8:9bb806a7f585 121 counter++;
pmic 6:e1fa1a2d7483 122
pmic 6:e1fa1a2d7483 123 /* visual feedback that the main task is executed */
pmic 6:e1fa1a2d7483 124 led = !led;
pmic 9:f10b974d01e0 125
pmic 1:93d997d6b232 126 } else {
pmic 6:e1fa1a2d7483 127
pmic 6:e1fa1a2d7483 128 dist = 0.0f;
pmic 1:93d997d6b232 129
pmic 9:f10b974d01e0 130 speedController_m0.setDesiredSpeedRPS(0.0f);
pmic 9:f10b974d01e0 131 speedController_m1.setDesiredSpeedRPS(0.0f);
pmic 9:f10b974d01e0 132 pwmOut_m2.write(0.5);
pmic 6:e1fa1a2d7483 133
pmic 8:9bb806a7f585 134 servoPeriod_mus_0 = 0;
pmic 8:9bb806a7f585 135 servoPeriod_mus_1 = 0;
pmic 8:9bb806a7f585 136 servo_0.SetPosition(servoPeriod_mus_0);
pmic 8:9bb806a7f585 137 servo_1.SetPosition(servoPeriod_mus_1);
pmic 1:93d997d6b232 138
pmic 6:e1fa1a2d7483 139 dist = analogIn.read() * 3.3f;
pmic 6:e1fa1a2d7483 140
pmic 6:e1fa1a2d7483 141 led = 0;
pmic 1:93d997d6b232 142 }
pmic 6:e1fa1a2d7483 143
pmic 8:9bb806a7f585 144 /* do only output what's really necessary*/
pmic 8:9bb806a7f585 145 printf("%3.3e, %3.3e, %3d, %3d; \r\n", speedController_m0.getSpeedRPS(),
pmic 9:f10b974d01e0 146 speedController_m1.getSpeedRPS(),
pmic 9:f10b974d01e0 147 servoPeriod_mus_0,
pmic 9:f10b974d01e0 148 servoPeriod_mus_1);
pmic 8:9bb806a7f585 149
pmic 6:e1fa1a2d7483 150 /* ------------- stop hacking ------------- -------------*/
pmic 6:e1fa1a2d7483 151
pmic 6:e1fa1a2d7483 152 int T_loop_ms = duration_cast<milliseconds>(loop_timer.elapsed_time()).count();
pmic 6:e1fa1a2d7483 153 int dT_loop_ms = Ts_ms - T_loop_ms;
pmic 6:e1fa1a2d7483 154 thread_sleep_for(dT_loop_ms);
pmic 1:93d997d6b232 155 }
pmic 1:93d997d6b232 156 }
pmic 6:e1fa1a2d7483 157
pmic 6:e1fa1a2d7483 158 void button_fall()
pmic 6:e1fa1a2d7483 159 {
pmic 6:e1fa1a2d7483 160 user_button_timer.reset();
pmic 6:e1fa1a2d7483 161 user_button_timer.start();
pmic 6:e1fa1a2d7483 162 }
pmic 6:e1fa1a2d7483 163
pmic 6:e1fa1a2d7483 164 void button_rise()
pmic 6:e1fa1a2d7483 165 {
pmic 6:e1fa1a2d7483 166 int t_button_ms = duration_cast<milliseconds>(user_button_timer.elapsed_time()).count();
pmic 6:e1fa1a2d7483 167 user_button_timer.stop();
pmic 8:9bb806a7f585 168 if (t_button_ms > 200) {
pmic 6:e1fa1a2d7483 169 executeMainTask = !executeMainTask;
pmic 8:9bb806a7f585 170 }
pmic 6:e1fa1a2d7483 171 }