Joel Balsiger / Mbed OS Senso_Test_3

Senso_3

main.cpp@0:5d4d21d56334, 2021-03-12 (annotated)

Committer:
boro
Date:
Fri Mar 12 13:04:33 2021 +0000
Revision:
0:5d4d21d56334
Child:
1:4e0e4d0363d9
controller added;

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 // Blinking rate in milliseconds
boro 0:5d4d21d56334 16 #define BLINKING_RATE_MS 500
boro 0:5d4d21d56334 17
boro 0:5d4d21d56334 18
boro 0:5d4d21d56334 19 int main()
boro 0:5d4d21d56334 20 {
boro 0:5d4d21d56334 21
boro 0:5d4d21d56334 22 DigitalIn user_button(USER_BUTTON);
boro 0:5d4d21d56334 23
boro 0:5d4d21d56334 24 // initialise PWM
boro 0:5d4d21d56334 25 PwmOut pwm_motor1(PB_13);
boro 0:5d4d21d56334 26 PwmOut pwm_motor2(PA_9);
boro 0:5d4d21d56334 27 PwmOut pwm_motor3(PA_10);
boro 0:5d4d21d56334 28
boro 0:5d4d21d56334 29 // crete Encoder read objects
boro 0:5d4d21d56334 30 EncoderCounter counter1(PA_6, PC_7); // counter(pin A, pin B)
boro 0:5d4d21d56334 31 EncoderCounter counter2(PB_6, PB_7);
boro 0:5d4d21d56334 32 EncoderCounter counter3(PA_0, PA_1);
boro 0:5d4d21d56334 33
boro 0:5d4d21d56334 34 // create controller
boro 0:5d4d21d56334 35 Controller controller(pwm_motor1, pwm_motor2, counter1, counter1);
boro 0:5d4d21d56334 36
boro 0:5d4d21d56334 37 DigitalOut enable(PB_15);
boro 0:5d4d21d56334 38
boro 0:5d4d21d56334 39 // create servo objects
boro 0:5d4d21d56334 40 Servo S0(PB_2);
boro 0:5d4d21d56334 41 Servo S1(PC_8);
boro 0:5d4d21d56334 42 Servo S2(PC_6);
boro 0:5d4d21d56334 43
boro 0:5d4d21d56334 44 SDBlockDevice sd(PC_12, PC_11, PC_10, PD_2);
boro 0:5d4d21d56334 45 printf("BlockDevice created\r\n");
boro 0:5d4d21d56334 46 FATFileSystem fs("fs", &sd);
boro 0:5d4d21d56334 47
boro 0:5d4d21d56334 48 // Initialise the digital pin LED1 as an output
boro 0:5d4d21d56334 49 DigitalOut myled(LED1);
boro 0:5d4d21d56334 50
boro 0:5d4d21d56334 51
boro 0:5d4d21d56334 52
boro 0:5d4d21d56334 53 // initialise PWM
boro 0:5d4d21d56334 54 pwm_motor1.period(0.00005f);// 0.05ms 20KHz
boro 0:5d4d21d56334 55 pwm_motor1.write(0.5f);
boro 0:5d4d21d56334 56 pwm_motor2.period(0.00005f);// 0.05ms 20KHz
boro 0:5d4d21d56334 57 pwm_motor2.write(0.5f);
boro 0:5d4d21d56334 58 pwm_motor3.period(0.00005f);// 0.05ms 20KHz
boro 0:5d4d21d56334 59 pwm_motor3.write(0.5f);
boro 0:5d4d21d56334 60
boro 0:5d4d21d56334 61 // initialise and test Servo
boro 0:5d4d21d56334 62 S0.Enable(1000,20000);
boro 0:5d4d21d56334 63 S1.Enable(1000,20000);
boro 0:5d4d21d56334 64 S2.Enable(1000,20000);
boro 0:5d4d21d56334 65
boro 0:5d4d21d56334 66 printf("Test writing... ");
boro 0:5d4d21d56334 67 FILE* fp = fopen("/fs/data.csv", "w");
boro 0:5d4d21d56334 68 fprintf(fp, "test %.5f\r\n",1.23);
boro 0:5d4d21d56334 69 fclose(fp);
boro 0:5d4d21d56334 70 printf("done\r\n");
boro 0:5d4d21d56334 71
boro 0:5d4d21d56334 72 printf("Test reading... ");
boro 0:5d4d21d56334 73 // read from SD card
boro 0:5d4d21d56334 74 fp = fopen("/fs/data.csv", "r");
boro 0:5d4d21d56334 75 if (fp != NULL) {
boro 0:5d4d21d56334 76 char c = fgetc(fp);
boro 0:5d4d21d56334 77 if (c == 't')
boro 0:5d4d21d56334 78 printf("done\r\n");
boro 0:5d4d21d56334 79 else
boro 0:5d4d21d56334 80 printf("incorrect char (%c)!\n", c);
boro 0:5d4d21d56334 81 fclose(fp);
boro 0:5d4d21d56334 82 } else {
boro 0:5d4d21d56334 83 printf("Reading failed!\n");
boro 0:5d4d21d56334 84 }
boro 0:5d4d21d56334 85
boro 0:5d4d21d56334 86 // enable driver DC motors
boro 0:5d4d21d56334 87 enable = 1;
boro 0:5d4d21d56334 88
boro 0:5d4d21d56334 89 while (true) {
boro 0:5d4d21d56334 90
boro 0:5d4d21d56334 91 if(!user_button) {
boro 0:5d4d21d56334 92 // LED off, set controller speed, pwm2, position servo
boro 0:5d4d21d56334 93 myled = 0;
boro 0:5d4d21d56334 94 controller.setDesiredSpeedLeft(50.0f);
boro 0:5d4d21d56334 95 controller.setDesiredSpeedRight(50.0f);
boro 0:5d4d21d56334 96 pwm_motor3.write(0.9f);
boro 0:5d4d21d56334 97
boro 0:5d4d21d56334 98 S0.SetPosition(1200);
boro 0:5d4d21d56334 99 S1.SetPosition(1200);
boro 0:5d4d21d56334 100 S2.SetPosition(1200);
boro 0:5d4d21d56334 101
boro 0:5d4d21d56334 102 } else {
boro 0:5d4d21d56334 103 // LED on, reset controller speed, pwm2, position servo
boro 0:5d4d21d56334 104 myled = 1;
boro 0:5d4d21d56334 105 controller.setDesiredSpeedLeft(0.0f);
boro 0:5d4d21d56334 106 controller.setDesiredSpeedRight(0.0f);
boro 0:5d4d21d56334 107 pwm_motor3.write(0.5f);
boro 0:5d4d21d56334 108
boro 0:5d4d21d56334 109 S0.SetPosition(1900);
boro 0:5d4d21d56334 110 S1.SetPosition(1900);
boro 0:5d4d21d56334 111 S2.SetPosition(1900);
boro 0:5d4d21d56334 112
boro 0:5d4d21d56334 113 }
boro 0:5d4d21d56334 114
boro 0:5d4d21d56334 115 printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read());
boro 0:5d4d21d56334 116
boro 0:5d4d21d56334 117 thread_sleep_for(BLINKING_RATE_MS);
boro 0:5d4d21d56334 118 }
boro 0:5d4d21d56334 119 }