Lucas Rosina / Mbed OS PM2_Example_PES_board_ros2

Workshop 2

Dependencies:   PM2_Libary

main.cpp@7:c0f5bb355f41, 2021-04-06 (annotated)

Committer:
pmic
Date:
Tue Apr 06 09:13:25 2021 +0200
Revision:
7:c0f5bb355f41
Parent:
6:e1fa1a2d7483
Child:
8:9bb806a7f585
Example with BufferedSerial.

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