first commit
Dependencies: PM2_Libary
Diff: main.cpp
- Revision:
- 3:d22942631cd7
- Parent:
- 2:7c03fefb77ef
- Child:
- 4:67506e285ad0
diff -r 7c03fefb77ef -r d22942631cd7 main.cpp --- a/main.cpp Thu Apr 01 11:18:01 2021 +0000 +++ b/main.cpp Thu Apr 01 14:31:52 2021 +0000 @@ -4,8 +4,7 @@ #include "FATFileSystem.h" #include "EncoderCounter.h" #include "Servo.h" -#include "Controller.h" - +#include "SpeedController.h" int main() { @@ -13,24 +12,25 @@ DigitalIn user_button(USER_BUTTON); // initialise PWM - PwmOut pwm_motor1(PB_13); - PwmOut pwm_motor2(PA_9); - PwmOut pwm_motor3(PA_10); + PwmOut pwm_motor_0(PB_13); + PwmOut pwm_motor_1(PA_9); + PwmOut pwm_motor_2(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); + EncoderCounter encoderCounter_0(PA_6, PC_7); + EncoderCounter encoderCounter_1(PB_6, PB_7); + EncoderCounter encoderCounter_2(PA_0, PA_1); // create controller - Controller controller(pwm_motor1, pwm_motor2, counter1, counter2); - - DigitalOut enable(PB_15); + 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 S0(PB_2); - Servo S1(PC_8); - Servo S2(PC_6); + Servo servo_0(PB_2); + Servo servo_1(PC_8); + Servo servo_2(PC_6); SDBlockDevice sd(PC_12, PC_11, PC_10, PD_2); printf("BlockDevice created\r\n"); @@ -39,22 +39,21 @@ // 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); + 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 - S0.Enable(1000,20000); // 1 ms / 20 ms - S1.Enable(1000,20000); - S2.Enable(1000,20000); - + servo_0.Enable(1000, 20000); // 1 ms / 20 ms + servo_1.Enable(1000, 20000); + servo_2.Enable(1000, 20000); int servo_desval_0 = 0; + int servo_desval_1 = 0; + int servo_desval_2 = 0; /* printf("Test writing... "); @@ -79,42 +78,47 @@ */ // enable driver DC motors - enable = 1; + enable_dc_motors = 1; while (true) { if(!user_button) { - // LED off, set controller speed, pwm2, position servo + // LED off, set speedController speed, pwm2, position servo myled = 0; - controller.setDesiredSpeedLeft(50.0f); - // controller.setDesiredSpeedRight(50.0f); - // pwm_motor1.write(0.7f); - pwm_motor3.write(0.7f); + + speedController_0.setDesiredSpeed( 60.0f); + speedController_1.setDesiredSpeed(-120.0f); + pwm_motor_2.write(0.7f); - - S0.SetPosition(servo_desval_0); - S1.SetPosition(1000); - S2.SetPosition(1000); - if(servo_desval_0 < 2000) servo_desval_0 += 100; + servo_0.SetPosition(servo_desval_0); + servo_1.SetPosition(servo_desval_1); + servo_2.SetPosition(servo_desval_2); + if(servo_desval_0 < 10000) servo_desval_0 += 100; + if(servo_desval_1 < 10000) servo_desval_1 += 100; + if(servo_desval_2 < 10000) servo_desval_2 += 100; } else { - // LED on, reset controller speed, pwm2, position servo + // LED on, reset speedController speed, pwm2, position servo myled = 1; - controller.setDesiredSpeedLeft(0.0f); - // controller.setDesiredSpeedRight(0.0f); - // pwm_motor1.write(0.5f); - pwm_motor3.write(0.5f); + + speedController_0.setDesiredSpeed(0.0f); + speedController_1.setDesiredSpeed(0.0f); + pwm_motor_2.write(0.5f); - servo_desval_0 = 0; - S0.SetPosition(servo_desval_0); - S1.SetPosition(1500); - S2.SetPosition(1500); + servo_desval_0 = 0.0f; + servo_desval_1 = 0.0f; + servo_desval_2 = 0.0f; + servo_0.SetPosition(servo_desval_0); + servo_1.SetPosition(servo_desval_1); + servo_2.SetPosition(servo_desval_2); } - - //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()); + // 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\r\n", static_cast<int>(speedController_0.getSpeed()*1000.0f), + static_cast<int>(speedController_1.getSpeed()*1000.0f), + encoderCounter_2.read()); thread_sleep_for(500); }