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