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