first commit
Dependencies: PM2_Libary
Diff: main.cpp
- Revision:
- 1:93d997d6b232
- Parent:
- 0:f44522e28559
- Child:
- 2:7c03fefb77ef
diff -r f44522e28559 -r 93d997d6b232 main.cpp --- a/main.cpp Tue Mar 30 12:50:43 2021 +0000 +++ b/main.cpp Tue Mar 30 14:20:41 2021 +0000 @@ -1,65 +1,118 @@ -#include "mbed.h" -#include "platform/mbed_thread.h" - -using namespace std::chrono; - -InterruptIn user_button(USER_BUTTON); -DigitalOut led(LED1); - -bool executeMainTask = false; -Timer user_button_timer, loop_timer; -int Ts_ms = 50; - -void button_fall(); -void button_rise(); - -/* input your stuff here */ -AnalogIn analogIn(PA_0); -float dist = 0.0f; - -int main() -{ - user_button.fall(&button_fall); - user_button.rise(&button_rise); - loop_timer.start(); - - while (true) { - - loop_timer.reset(); - - /* ------------- start hacking ------------- -------------*/ - - if(executeMainTask) { - - dist = analogIn.read()*3.3f; - - /* do only output what's really necessary, outputting "Measured value in mV: "" within the loop is no good solution */ - printf("Measured value in mV: %d\r\n", (static_cast<int>(dist * 1e3))); - - /* visual feedback that the main task is executed */ - led = !led; - - } else { - led = 0; - } - - /* ------------- stop hacking ------------- -------------*/ - - int T_loop_ms = duration_cast<milliseconds>(loop_timer.elapsed_time()).count(); - int dT_loop_ms = Ts_ms - T_loop_ms; - thread_sleep_for(dT_loop_ms); - } -} - -void button_fall() -{ - user_button_timer.reset(); - user_button_timer.start(); -} - -void button_rise() -{ - int t_button = duration_cast<milliseconds>(user_button_timer.elapsed_time()).count(); - user_button_timer.stop(); - if(t_button > 200) executeMainTask = !executeMainTask; -} \ No newline at end of file +/* mbed Microcontroller Library + * Copyright (c) 2019 ARM Limited + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "mbed.h" +#include "platform/mbed_thread.h" +#include "SDBlockDevice.h" +#include "FATFileSystem.h" +#include "EncoderCounter.h" +#include "Servo.h" +#include "Controller.h" + + +int main() +{ + + DigitalIn user_button(USER_BUTTON); + + // initialise PWM + PwmOut pwm_motor1(PB_13); + PwmOut pwm_motor2(PA_9); + PwmOut pwm_motor3(PA_10); + + // crete Encoder read objects + EncoderCounter counter1(PA_6, PC_7); // counter(pin A, pin B) + EncoderCounter counter2(PB_6, PB_7); + EncoderCounter counter3(PA_0, PA_1); + + // create controller + Controller controller(pwm_motor1, pwm_motor2, counter1, counter2); + + DigitalOut enable(PB_15); + + // create servo objects + Servo S0(PB_2); + Servo S1(PC_8); + Servo S2(PC_6); + + SDBlockDevice sd(PC_12, PC_11, PC_10, PD_2); + printf("BlockDevice created\r\n"); + FATFileSystem fs("fs", &sd); + + // Initialise the digital pin LED1 as an output + DigitalOut myled(LED1); + + + /* + // initialise PWM + pwm_motor1.period(0.00005f);// 0.05ms 20KHz + pwm_motor1.write(0.5f); + pwm_motor2.period(0.00005f);// 0.05ms 20KHz + pwm_motor2.write(0.5f);*/ + pwm_motor3.period(0.00005f);// 0.05ms 20KHz + pwm_motor3.write(0.5f); + + // initialise and test Servo + S0.Enable(1000,20000); + S1.Enable(1000,20000); + S2.Enable(1000,20000); + + printf("Test writing... "); + FILE* fp = fopen("/fs/data.csv", "w"); + fprintf(fp, "test %.5f\r\n",1.23); + fclose(fp); + printf("done\r\n"); + + printf("Test reading... "); + // read from SD card + fp = fopen("/fs/data.csv", "r"); + if (fp != NULL) { + char c = fgetc(fp); + if (c == 't') + printf("done\r\n"); + else + printf("incorrect char (%c)!\n", c); + fclose(fp); + } else { + printf("Reading failed!\n"); + } + + // enable driver DC motors + enable = 1; + + while (true) { + + if(!user_button) { + // LED off, set controller speed, pwm2, position servo + myled = 0; + controller.setDesiredSpeedLeft(50.0f); + controller.setDesiredSpeedRight(50.0f); + pwm_motor3.write(0.7f); + + + S0.SetPosition(1000); + S1.SetPosition(1000); + S2.SetPosition(1000); + + } else { + // LED on, reset controller speed, pwm2, position servo + myled = 1; + controller.setDesiredSpeedLeft(0.0f); + controller.setDesiredSpeedRight(0.0f); + pwm_motor3.write(0.5f); + + S0.SetPosition(1500); + S1.SetPosition(1500); + S2.SetPosition(1500); + + } + + + printf("speedLeft: %f, speedRight: %f\r\n",controller.getSpeedLeft(), controller.getSpeedRight()); + //printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read()); + + thread_sleep_for(200); + } +}