with the tof code
Dependencies: mbed Servo ros_lib_kinetic
Motors/Motor.cpp
- Committer:
- Stumi
- Date:
- 2019-11-07
- Revision:
- 4:36a04230554d
- Parent:
- 3:df6160e2f6d9
- Child:
- 6:2cc2aac35868
File content as of revision 4:36a04230554d:
/*-------------------------------------------------------------------------------- Filename: main.cpp Description: Main program loop --------------------------------------------------------------------------------*/ #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) { // Set initial condition of PWM _pwm.period(0.001); //1KHz _pwm = 0; // Initial condition of output enables _fwd = 0; _rev = 0; } /*-------------------------------------------------------------------------------- Function name: Forwards Input Parameters: float speed - PWM duty between 0-1 Output Parameters: N/A Description: Drives the motor forwards at a designated speed ----------------------------------------------------------------------------------*/ void cMotor::Forwards(float speed) { _fwd = 1; _rev = 0; _pwm = speed; } /*-------------------------------------------------------------------------------- Function name: Backwards Input Parameters: float speed - PWM duty between 0-1 Output Parameters: N/A Description: Drives the motor backwards at a designated speed ----------------------------------------------------------------------------------*/ void cMotor::Backwards(float speed) { _fwd = 0; _rev = 1; _pwm = speed; } /*-------------------------------------------------------------------------------- Function name: Stop Input Parameters: N/A Output Parameters: N/A Description: Drives the motor backwards at a designated speed ----------------------------------------------------------------------------------*/ void cMotor::Stop() { _fwd = 0; _rev = 0; _pwm = 0; }