first commit
Dependencies: PM2_Libary
Diff: main.cpp
- Revision:
- 6:e1fa1a2d7483
- Parent:
- 4:67506e285ad0
- Child:
- 7:c0f5bb355f41
diff -r 9f955eef2059 -r e1fa1a2d7483 main.cpp --- a/main.cpp Fri Apr 02 09:33:18 2021 +0200 +++ b/main.cpp Tue Apr 06 08:00:43 2021 +0200 @@ -1,69 +1,91 @@ #include "mbed.h" #include "platform/mbed_thread.h" -#include "SDBlockDevice.h" -#include "FATFileSystem.h" +#include "string" + +/* PM2_Libary */ #include "EncoderCounter.h" #include "Servo.h" #include "SpeedController.h" +// #include "FATFileSystem.h" +// #include "SDBlockDevice.h" + +using namespace std::chrono; + +InterruptIn user_button(USER_BUTTON); +DigitalOut led(LED1); +BufferedSerial bufferedSerial(USBTX, USBRX); +char buffer[100]; + +bool executeMainTask = false; +Timer user_button_timer, loop_timer; +int Ts_ms = 2; + +/* declaration of custom button functions */ +void button_fall(); +void button_rise(); + +/* create analog input object */ +AnalogIn analogIn(PC_2); +float dist = 0.0f; + +/* create pwm objects */ +PwmOut pwmOut_m0(PB_13); +PwmOut pwmOut_m1(PA_9); +PwmOut pwmOut_m2(PA_10); +float Ts_pwm = 0.00005f; +/* create enable dc motor digital out object */ +DigitalOut enable_motors(PB_15); +/* create encoder read objects */ +EncoderCounter encoderCounter_m0(PA_6, PC_7); +EncoderCounter encoderCounter_m1(PB_6, PB_7); +EncoderCounter encoderCounter_m2(PA_0, PA_1); +/* create speed controller objects, only m0 and m1, m2 is used open-loop */ +SpeedController speedController_m0(1562.5f, 15.0f, 0.1f, 12.0f, pwmOut_m0, encoderCounter_m0); +SpeedController speedController_m1(1562.5f, 15.0f, 0.1f, 12.0f, pwmOut_m1, encoderCounter_m1); + +/* create servo objects */ +Servo servo_0(PB_2); +Servo servo_1(PC_8); +Servo servo_2(PC_6); // not used in this example +int Ts_servo_mus = 20000; +int servo_desval_0 = 0; +int servo_desval_1 = 0; + +/* create sd object */ +// SDBlockDevice sd(PC_12, PC_11, PC_10, PD_2); +// FATFileSystem fs("fs", &sd); int main() { - - DigitalIn user_button(USER_BUTTON); - - // initialise PWM - PwmOut pwm_motor_0(PB_13); - PwmOut pwm_motor_1(PA_9); - PwmOut pwm_motor_2(PA_10); - - // crete Encoder read objects - EncoderCounter encoderCounter_0(PA_6, PC_7); - EncoderCounter encoderCounter_1(PB_6, PB_7); - EncoderCounter encoderCounter_2(PA_0, PA_1); - - // create controller - SpeedController speedController_0(pwm_motor_0, encoderCounter_0); - SpeedController speedController_1(pwm_motor_1, encoderCounter_1); - - DigitalOut enable_dc_motors(PB_15); - - // create servo objects - Servo servo_0(PB_2); - Servo servo_1(PC_8); - Servo servo_2(PC_6); + bufferedSerial.set_baud(115200); - 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); + user_button.fall(&button_fall); + user_button.rise(&button_rise); + loop_timer.start(); + + /* initialize pwm */ + pwmOut_m0.period(Ts_pwm); + pwmOut_m1.period(Ts_pwm); + pwmOut_m2.period(Ts_pwm); + /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max */ + pwmOut_m1.write(0.5f); + pwmOut_m0.write(0.5f); + pwmOut_m2.write(0.5f); + /* enable driver DC motors */ + enable_motors = 1; + + /* initialize servo */ + servo_0.Enable(servo_desval_0, Ts_servo_mus); // 1 ms / 20 ms + servo_1.Enable(servo_desval_0, Ts_servo_mus); - // initialise PWM - pwm_motor_0.period(0.00005f);// 0.05ms 20KHz - pwm_motor_0.write(0.5f); - pwm_motor_1.period(0.00005f);// 0.05ms 20KHz - pwm_motor_1.write(0.5f); - pwm_motor_2.period(0.00005f);// 0.05ms 20KHz - pwm_motor_2.write(0.5f); - - // initialise and test Servo - servo_0.Enable(1000, 20000); // 1 ms / 20 ms - servo_1.Enable(1000, 20000); - int servo_desval_0 = 0; - int servo_desval_1 = 0; - - /* input your stuff here */ - AnalogIn analogIn(PC_2); - float dist = 0.0f; - /* + // example code for sd card, not tested from pmic, 02.04.2021 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"); @@ -78,51 +100,75 @@ printf("Reading failed!\n"); } */ - - // enable driver DC motors - enable_dc_motors = 1; - + while (true) { - - if(!user_button) { - // LED off, set speedController speed, pwm2, position servo - myled = 0; - - speedController_0.setDesiredSpeed( 60.0f); - speedController_1.setDesiredSpeed(-120.0f); - pwm_motor_2.write(0.7f); - + + loop_timer.reset(); + + /* ------------- start hacking ------------- -------------*/ + + if (executeMainTask) { + + /* read analog input */ + dist = analogIn.read() * 3.3f; + + speedController_m0.setDesiredSpeedRPS( 1.0f); + speedController_m1.setDesiredSpeedRPS(-2.0f); + pwmOut_m2.write(0.75f); + servo_0.SetPosition(servo_desval_0); servo_1.SetPosition(servo_desval_1); - if(servo_desval_0 < 10000) servo_desval_0 += 100; - if(servo_desval_1 < 10000) servo_desval_1 += 100; + if (servo_desval_0 < 10000) + servo_desval_0 += 100; + if (servo_desval_1 < 10000) + servo_desval_1 += 100; + + /* visual feedback that the main task is executed */ + led = !led; + + /* write output via serial buffer */ + int act_buffer_length = snprintf (buffer, 100, + "%3.6e, %3.6e; \r\n", + speedController_m0.getSpeedRPM(), + speedController_m1.getSpeedRPM()); + bufferedSerial.write(buffer, act_buffer_length); - dist = analogIn.read()*3.3f; - } else { - // LED on, reset speedController speed, pwm2, position servo - myled = 1; - - speedController_0.setDesiredSpeed(0.0f); - speedController_1.setDesiredSpeed(0.0f); - pwm_motor_2.write(0.5f); + + dist = 0.0f; - servo_desval_0 = 0.0f; - servo_desval_1 = 0.0f; + speedController_m0.setDesiredSpeedRPS(0.0f); + speedController_m1.setDesiredSpeedRPS(0.0f); + pwmOut_m2.write(0.5f); + + servo_desval_0 = 0; + servo_desval_1 = 0; servo_0.SetPosition(servo_desval_0); servo_1.SetPosition(servo_desval_1); - - dist = analogIn.read()*3.3f; + dist = analogIn.read() * 3.3f; + + led = 0; } - - // printf("speedLeft: %f, speedRight: %f\r\n",controller.getSpeedLeft(), controller.getSpeedRight()); - // printf("counter1 = %d counter2 = %d counter3 = %d\r\n", encoderCounter_0.read(), encoderCounter_1.read(), encoderCounter_2.read()); - printf("speedController_0 = %d speedController_1 = %d encoderCounter_2 = %d measured value in mV: %d\r\n", static_cast<int>(speedController_0.getSpeed()*1000.0f), - static_cast<int>(speedController_1.getSpeed()*1000.0f), - encoderCounter_2.read(), - (static_cast<int>(dist * 1e3))); - - thread_sleep_for(500); + + /* ------------- 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_ms = duration_cast<milliseconds>(user_button_timer.elapsed_time()).count(); + user_button_timer.stop(); + if (t_button_ms > 1) + executeMainTask = !executeMainTask; +} \ No newline at end of file