Nicolas Borla / Mbed OS PES2_mbed_os

PES2_mbed_os_6

Dependencies:   Servo

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?

UserRevisionLine numberNew 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 }