Fabian Käser / Mbed OS PM2_Example_PES_board

Motors

Dependencies:   PM2_Libary

main.cpp@11:fc0102f9c68d, 2021-04-12 (annotated)

Committer:
fabian5
Date:
Mon Apr 12 08:46:52 2021 +0000
Revision:
11:fc0102f9c68d
Parent:
10:c5d85e35758c 
Motors

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
pmic 6:e1fa1a2d7483 10 using namespace std::chrono;
pmic 6:e1fa1a2d7483 11
pmic 8:9bb806a7f585 12 InterruptIn user_button(USER_BUTTON);
pmic 8:9bb806a7f585 13 DigitalOut led(LED1);
pmic 6:e1fa1a2d7483 14
pmic 6:e1fa1a2d7483 15 bool executeMainTask = false;
pmic 6:e1fa1a2d7483 16 Timer user_button_timer, loop_timer;
pmic 7:c0f5bb355f41 17 int Ts_ms = 50;
pmic 6:e1fa1a2d7483 18
pmic 6:e1fa1a2d7483 19 /* declaration of custom button functions */
pmic 6:e1fa1a2d7483 20 void button_fall();
pmic 6:e1fa1a2d7483 21 void button_rise();
pmic 6:e1fa1a2d7483 22
pmic 6:e1fa1a2d7483 23 /* create analog input object */
pmic 6:e1fa1a2d7483 24 AnalogIn analogIn(PC_2);
pmic 6:e1fa1a2d7483 25 float dist = 0.0f;
pmic 6:e1fa1a2d7483 26
pmic 6:e1fa1a2d7483 27 /* create enable dc motor digital out object */
pmic 6:e1fa1a2d7483 28 DigitalOut enable_motors(PB_15);
pmic 10:c5d85e35758c 29 /* create pwm objects */
pmic 10:c5d85e35758c 30 FastPWM pwmOut_M1(PB_13);
pmic 10:c5d85e35758c 31 FastPWM pwmOut_M2(PA_9);
pmic 10:c5d85e35758c 32 FastPWM pwmOut_M3(PA_10);
pmic 10:c5d85e35758c 33 double Ts_pwm_s = 0.00005; // this needs to be a double value (no f at the end)
pmic 6:e1fa1a2d7483 34 /* create encoder read objects */
pmic 10:c5d85e35758c 35 EncoderCounter encoderCounter_M1(PA_6, PC_7);
pmic 10:c5d85e35758c 36 EncoderCounter encoderCounter_M2(PB_6, PB_7);
pmic 10:c5d85e35758c 37 EncoderCounter encoderCounter_M3(PA_0, PA_1);
pmic 10:c5d85e35758c 38 /* create speed controller objects, only M1 and M2, M3 is used open-loop */
pmic 10:c5d85e35758c 39 float counts_per_turn = 20.0f*78.125f; // counts/turn * gearratio
pmic 10:c5d85e35758c 40 float kn = 180.0f/12.0f; // (RPM/V)
pmic 10:c5d85e35758c 41 float max_voltage = 12.0f; // adjust this to 6.0f if only one batterypack is used
pmic 10:c5d85e35758c 42 SpeedController speedController_M1(counts_per_turn, kn, max_voltage, pwmOut_M1, encoderCounter_M1);
pmic 10:c5d85e35758c 43 SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwmOut_M2, encoderCounter_M2);
fabian5 11:fc0102f9c68d 44 SpeedController speedController_M3(counts_per_turn, kn, max_voltage, pwmOut_M3, encoderCounter_M3);
pmic 6:e1fa1a2d7483 45
pmic 6:e1fa1a2d7483 46 /* create servo objects */
pmic 10:c5d85e35758c 47 Servo servo_S1(PB_2);
pmic 10:c5d85e35758c 48 Servo servo_S2(PC_8);
fabian5 11:fc0102f9c68d 49 Servo servo_S3(PC_6); // not needed in this example
pmic 10:c5d85e35758c 50 int servoPeriod_mus = 20000;
pmic 10:c5d85e35758c 51 int servoOutput_mus_S1 = 0;
pmic 10:c5d85e35758c 52 int servoOutput_mus_S2 = 0;
fabian5 11:fc0102f9c68d 53 int servoOutput_mus_S3 = 0;
pmic 10:c5d85e35758c 54 int servo_counter = 0;
pmic 10:c5d85e35758c 55 int loops_per_second = static_cast<int>(ceilf(1.0f/(0.001f*(float)Ts_ms)));
pmic 1:93d997d6b232 56
pmic 1:93d997d6b232 57 int main()
pmic 9:f10b974d01e0 58 {
pmic 6:e1fa1a2d7483 59 user_button.fall(&button_fall);
pmic 6:e1fa1a2d7483 60 user_button.rise(&button_rise);
pmic 6:e1fa1a2d7483 61 loop_timer.start();
pmic 6:e1fa1a2d7483 62
pmic 10:c5d85e35758c 63 /* enable hardwaredriver dc motors */
pmic 10:c5d85e35758c 64 enable_motors = 1;
pmic 10:c5d85e35758c 65 /* initialize pwm for motor M3*/
pmic 10:c5d85e35758c 66 pwmOut_M3.period(Ts_pwm_s);
pmic 6:e1fa1a2d7483 67 /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max */
pmic 10:c5d85e35758c 68 pwmOut_M3.write(0.5);
pmic 9:f10b974d01e0 69
pmic 10:c5d85e35758c 70 /* enable servos, you can also disable them */
pmic 10:c5d85e35758c 71 servo_S1.Enable(servoOutput_mus_S1, servoPeriod_mus);
pmic 10:c5d85e35758c 72 servo_S2.Enable(servoOutput_mus_S2, servoPeriod_mus);
fabian5 11:fc0102f9c68d 73 servo_S3.Enable(servoOutput_mus_S3, servoPeriod_mus);
pmic 6:e1fa1a2d7483 74
pmic 1:93d997d6b232 75 while (true) {
pmic 6:e1fa1a2d7483 76
pmic 6:e1fa1a2d7483 77 loop_timer.reset();
pmic 6:e1fa1a2d7483 78
pmic 6:e1fa1a2d7483 79 /* ------------- start hacking ------------- -------------*/
pmic 6:e1fa1a2d7483 80
pmic 6:e1fa1a2d7483 81 if (executeMainTask) {
pmic 6:e1fa1a2d7483 82
pmic 6:e1fa1a2d7483 83 /* read analog input */
pmic 6:e1fa1a2d7483 84 dist = analogIn.read() * 3.3f;
pmic 6:e1fa1a2d7483 85
pmic 10:c5d85e35758c 86 /* command a speed to dc motors M1 and M2*/
pmic 10:c5d85e35758c 87 speedController_M1.setDesiredSpeedRPS( 1.0f);
pmic 10:c5d85e35758c 88 speedController_M2.setDesiredSpeedRPS(-0.5f);
fabian5 11:fc0102f9c68d 89 speedController_M3.setDesiredSpeedRPS(-0.5f);
pmic 10:c5d85e35758c 90 /* write output voltage to motor M3 */
pmic 10:c5d85e35758c 91 pwmOut_M3.write(0.75);
pmic 6:e1fa1a2d7483 92
pmic 10:c5d85e35758c 93 /* command servo position via output time, this needs to be calibrated */
pmic 10:c5d85e35758c 94 servo_S1.SetPosition(servoOutput_mus_S1);
pmic 10:c5d85e35758c 95 servo_S2.SetPosition(servoOutput_mus_S2);
fabian5 11:fc0102f9c68d 96 servo_S3.SetPosition(servoOutput_mus_S3);
pmic 10:c5d85e35758c 97 if (servoOutput_mus_S1 <= servoPeriod_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
pmic 10:c5d85e35758c 98 servoOutput_mus_S1 += 100;
pmic 8:9bb806a7f585 99 }
pmic 10:c5d85e35758c 100 if (servoOutput_mus_S2 <= servoPeriod_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
pmic 10:c5d85e35758c 101 servoOutput_mus_S2 += 100;
pmic 8:9bb806a7f585 102 }
fabian5 11:fc0102f9c68d 103 if (servoOutput_mus_S3 <= servoPeriod_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
fabian5 11:fc0102f9c68d 104 servoOutput_mus_S3 += 100;
fabian5 11:fc0102f9c68d 105 }
pmic 10:c5d85e35758c 106 servo_counter++;
pmic 6:e1fa1a2d7483 107
pmic 6:e1fa1a2d7483 108 /* visual feedback that the main task is executed */
pmic 6:e1fa1a2d7483 109 led = !led;
pmic 9:f10b974d01e0 110
pmic 1:93d997d6b232 111 } else {
pmic 6:e1fa1a2d7483 112
pmic 6:e1fa1a2d7483 113 dist = 0.0f;
pmic 1:93d997d6b232 114
pmic 10:c5d85e35758c 115 speedController_M1.setDesiredSpeedRPS(0.0f);
pmic 10:c5d85e35758c 116 speedController_M2.setDesiredSpeedRPS(0.0f);
fabian5 11:fc0102f9c68d 117 speedController_M2.setDesiredSpeedRPS(0.0f);
pmic 10:c5d85e35758c 118 pwmOut_M3.write(0.5);
pmic 6:e1fa1a2d7483 119
pmic 10:c5d85e35758c 120 servoOutput_mus_S1 = 0;
pmic 10:c5d85e35758c 121 servoOutput_mus_S2 = 0;
fabian5 11:fc0102f9c68d 122 servoOutput_mus_S3 = 0;
pmic 10:c5d85e35758c 123 servo_S1.SetPosition(servoOutput_mus_S1);
pmic 10:c5d85e35758c 124 servo_S2.SetPosition(servoOutput_mus_S2);
fabian5 11:fc0102f9c68d 125 servo_S3.SetPosition(servoOutput_mus_S3);
pmic 6:e1fa1a2d7483 126
pmic 6:e1fa1a2d7483 127 led = 0;
pmic 1:93d997d6b232 128 }
pmic 6:e1fa1a2d7483 129
pmic 10:c5d85e35758c 130 /* do only output via serial what's really necessary (this makes your code slow)*/
fabian5 11:fc0102f9c68d 131 printf("%3.3f, %3d, %3d, %3d, %3.3f, %3.3f, %3.3f;\r\n",
pmic 10:c5d85e35758c 132 dist,
pmic 10:c5d85e35758c 133 servoOutput_mus_S1,
pmic 10:c5d85e35758c 134 servoOutput_mus_S2,
fabian5 11:fc0102f9c68d 135 servoOutput_mus_S3,
pmic 10:c5d85e35758c 136 encoderCounter_M3.read(),
pmic 10:c5d85e35758c 137 speedController_M1.getSpeedRPS(),
fabian5 11:fc0102f9c68d 138 speedController_M2.getSpeedRPS(),
fabian5 11:fc0102f9c68d 139 speedController_M3.getSpeedRPS());
pmic 8:9bb806a7f585 140
pmic 6:e1fa1a2d7483 141 /* ------------- stop hacking ------------- -------------*/
pmic 6:e1fa1a2d7483 142
pmic 6:e1fa1a2d7483 143 int T_loop_ms = duration_cast<milliseconds>(loop_timer.elapsed_time()).count();
pmic 6:e1fa1a2d7483 144 int dT_loop_ms = Ts_ms - T_loop_ms;
pmic 6:e1fa1a2d7483 145 thread_sleep_for(dT_loop_ms);
pmic 1:93d997d6b232 146 }
pmic 1:93d997d6b232 147 }
pmic 6:e1fa1a2d7483 148
pmic 6:e1fa1a2d7483 149 void button_fall()
pmic 6:e1fa1a2d7483 150 {
pmic 6:e1fa1a2d7483 151 user_button_timer.reset();
pmic 6:e1fa1a2d7483 152 user_button_timer.start();
pmic 6:e1fa1a2d7483 153 }
pmic 6:e1fa1a2d7483 154
pmic 6:e1fa1a2d7483 155 void button_rise()
pmic 6:e1fa1a2d7483 156 {
pmic 6:e1fa1a2d7483 157 int t_button_ms = duration_cast<milliseconds>(user_button_timer.elapsed_time()).count();
pmic 6:e1fa1a2d7483 158 user_button_timer.stop();
pmic 8:9bb806a7f585 159 if (t_button_ms > 200) {
pmic 6:e1fa1a2d7483 160 executeMainTask = !executeMainTask;
pmic 8:9bb806a7f585 161 }
pmic 6:e1fa1a2d7483 162 }