with the tof code
Dependencies: mbed Servo ros_lib_kinetic
Diff: Motors/Motor.cpp
- Revision:
- 4:36a04230554d
- Parent:
- 3:df6160e2f6d9
- Child:
- 6:2cc2aac35868
diff -r df6160e2f6d9 -r 36a04230554d Motors/Motor.cpp --- a/Motors/Motor.cpp Thu Oct 24 14:37:51 2019 +0000 +++ b/Motors/Motor.cpp Thu Nov 07 15:31:52 2019 +0000 @@ -5,14 +5,88 @@ #include "Motor.h" +cMotor MotorL(M1_PWM, M1_IN1, M1_IN2); //Left motor class and pin initialisation +cMotor MotorR(M2_PWM, M2_IN1, M2_IN2); //Right motor class and pin initialisation + +/*-------------------------------------------------------------------------------- +Function name: Check_for_obstacles +Input Parameters: cMotor - MotorL (left motor) cMotor MotorR (right motor) TOFrange[4] - sensor measurements +Output Parameters: N/A +Description: Simple obstacle avoidance functionality +----------------------------------------------------------------------------------*/ +void Check_for_obstacles(uint8_t TOFRange[4]) +{ + //If top right sensor(6) detects something + if (TOFRange[2]<200) { + + if(TOFRange[2]>150) { //Provided its 15cm away... + + if(TOFRange[3]<200) { //...and the back sensor detects something + //Smooth turn right + MotorR.Forwards(0.8f); + MotorL.Forwards(0.2f); + + } else if(TOFRange[1]<200) { //...and the top left sensor detects something + //Smooth turn left + MotorR.Forwards(0.2f); + MotorL.Forwards(0.8f); + + } else { + MotorR.Forwards(0.8f); + MotorL.Forwards(0.2f); + } + + } else { + + if(TOFRange[3]<200) { + MotorR.Backwards(0.1f); + MotorL.Backwards(0.9f); + + } else if(TOFRange[1]<200) { + MotorR.Backwards(0.9f); + MotorL.Backwards(0.1f); + + } else { + MotorR.Backwards(0.1f); + MotorL.Backwards(0.9f); + } + } + + } else if(TOFRange[3]<200) { + + if(TOFRange[1]<200) { + MotorR.Forwards(1.0f); + MotorL.Forwards(1.0f); + } else { + + MotorR.Forwards(0.9f); + MotorL.Forwards(0.1f); + } + + } else if(TOFRange[1]<200) { + MotorR.Forwards(0.1f); + MotorL.Forwards(0.9f); + + } else if(TOFRange[0]<200) { + MotorR.Forwards(1.0f); + MotorL.Forwards(1.0f); + } + + else { + MotorR.Forwards(1.0f); + MotorL.Forwards(1.0f); + } +} + /*-------------------------------------------------------------------------------- Function name: cMotor Input Parameters: PwmOut pwm - Motor PWM, DigitalOut fwd - Motor input1, DigitalOut rev - Motor Input2 Output Parameters: N/A Description: Class constructor (Initialisation upon creating class) ----------------------------------------------------------------------------------*/ -cMotor::cMotor(PwmOut pwm, DigitalOut fwd, DigitalOut rev):_pwm(pwm), _fwd(fwd), _rev(rev){ - +cMotor::cMotor(PwmOut pwm, DigitalOut fwd, DigitalOut rev):_pwm(pwm), _fwd(fwd), _rev(rev) +{ + // Set initial condition of PWM _pwm.period(0.001); //1KHz _pwm = 0; @@ -28,7 +102,8 @@ Output Parameters: N/A Description: Drives the motor forwards at a designated speed ----------------------------------------------------------------------------------*/ -void cMotor::Forwards(float speed){ +void cMotor::Forwards(float speed) +{ _fwd = 1; _rev = 0; _pwm = speed; @@ -40,7 +115,8 @@ Output Parameters: N/A Description: Drives the motor backwards at a designated speed ----------------------------------------------------------------------------------*/ -void cMotor::Backwards(float speed){ +void cMotor::Backwards(float speed) +{ _fwd = 0; _rev = 1; _pwm = speed; @@ -52,7 +128,8 @@ Output Parameters: N/A Description: Drives the motor backwards at a designated speed ----------------------------------------------------------------------------------*/ -void cMotor::Stop(){ +void cMotor::Stop() +{ _fwd = 0; _rev = 0; _pwm = 0;