main.cpp

00001 /* mbed Microcontroller Library
00002  * Copyright (c) 2019 ARM Limited
00003  * SPDX-License-Identifier: Apache-2.0
00004  */
00005 
00006 #include "mbed.h"
00007 #include "platform/mbed_thread.h"
00008 #include "SDBlockDevice.h"
00009 #include "FATFileSystem.h"
00010 #include "EncoderCounter.h"
00011 #include "Servo.h"
00012 #include "Controller.h"
00013 
00014 
00015 int main()
00016 {
00017     
00018     DigitalIn user_button(USER_BUTTON);
00019     
00020     // initialise PWM
00021     PwmOut pwm_motor1(PB_13);
00022     PwmOut pwm_motor2(PA_9);
00023     PwmOut pwm_motor3(PA_10);
00024     
00025     // crete Encoder read objects
00026     EncoderCounter counter1(PA_6, PC_7); // counter(pin A, pin B)
00027     EncoderCounter counter2(PB_6, PB_7);
00028     EncoderCounter counter3(PA_0, PA_1);
00029     
00030     // create controller
00031     Controller controller(pwm_motor1, pwm_motor2, counter1, counter2);
00032     
00033     DigitalOut enable(PB_15);
00034     
00035     // create servo objects
00036     Servo  S0(PB_2);
00037     Servo  S1(PC_8);
00038     Servo  S2(PC_6);
00039     
00040     SDBlockDevice sd(PC_12, PC_11, PC_10, PD_2);
00041     printf("BlockDevice created\r\n"); 
00042     FATFileSystem fs("fs", &sd);
00043     
00044     // Initialise the digital pin LED1 as an output
00045     DigitalOut myled(LED1);
00046     
00047     
00048     /*
00049     // initialise PWM
00050     pwm_motor1.period(0.00005f);// 0.05ms 20KHz
00051     pwm_motor1.write(0.5f);
00052     pwm_motor2.period(0.00005f);// 0.05ms 20KHz
00053     pwm_motor2.write(0.5f);*/
00054     pwm_motor3.period(0.00005f);// 0.05ms 20KHz
00055     pwm_motor3.write(0.5f);
00056     
00057     // initialise and test Servo
00058     S0.Enable(1000,20000);
00059     S1.Enable(1000,20000);
00060     S2.Enable(1000,20000);
00061     
00062     printf("Test writing... ");
00063     FILE* fp = fopen("/fs/data.csv", "w");
00064     fprintf(fp, "test %.5f\r\n",1.23);
00065     fclose(fp);
00066     printf("done\r\n");
00067     
00068     printf("Test reading... ");
00069     // read from SD card
00070     fp = fopen("/fs/data.csv", "r");
00071     if (fp != NULL) {
00072         char c = fgetc(fp);
00073         if (c == 't')
00074             printf("done\r\n");
00075         else
00076             printf("incorrect char (%c)!\n", c);
00077         fclose(fp);
00078     } else {
00079         printf("Reading failed!\n");
00080     }
00081     
00082     // enable driver DC motors
00083     enable = 1;
00084     
00085     while (true) {
00086         
00087         if(!user_button) {
00088             // LED off, set controller speed, pwm2, position servo
00089             myled = 0;
00090             controller.setDesiredSpeedLeft(50.0f);
00091             controller.setDesiredSpeedRight(50.0f);
00092             pwm_motor3.write(0.7f);
00093             
00094 
00095             S0.SetPosition(1000);
00096             S1.SetPosition(1000);
00097             S2.SetPosition(1000);
00098 
00099         } else {
00100             // LED on, reset controller speed, pwm2, position servo
00101             myled = 1;
00102             controller.setDesiredSpeedLeft(0.0f);
00103             controller.setDesiredSpeedRight(0.0f);
00104             pwm_motor3.write(0.5f);
00105 
00106             S0.SetPosition(1500);
00107             S1.SetPosition(1500);
00108             S2.SetPosition(1500);
00109 
00110         }
00111 
00112         
00113         printf("speedLeft: %f, speedRight: %f\r\n",controller.getSpeedLeft(), controller.getSpeedRight());
00114         //printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read());
00115         
00116         thread_sleep_for(200);
00117     }
00118 }