Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed Servo ros_lib_kinetic
Motors/Motor.cpp
- Committer:
- hongyunAHN
- Date:
- 2020-01-05
- Revision:
- 10:276cc357015c
- Parent:
- 9:326b8f261ef0
File content as of revision 10:276cc357015c:
/*-------------------------------------------------------------------------------- Filename: main.cpp Description: Main program loop --------------------------------------------------------------------------------*/ #include "main.h" #include "Buzzer.h" #include "Motor.h" #include "LED.h" #include "TOF.h" #include <ros.h> #include <Servo.h> #include <std_msgs/UInt8.h> #include "mbed.h" //#include <std_msgs/UInt16.h> DigitalOut TOF1(PC_8); DigitalOut TOF3(PC_11); DigitalOut TOF5(PC_12); DigitalOut TOF7(PD_2); cAdafruit_VL6180X VL6180X(TOF1,TOF3,TOF5,TOF7); 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 //Servo servo1(srvoTilt); //Servo servo2(srvoPan); //Class definitions cBuzzer cBuzzer(Buzz); cRGB_LED cRGB_LED(DIAG_RED, DIAG_BLU, DIAG_GRN); uint8_t serviceTOF(uint8_t address){ uint8_t range = 0; // poll the VL6180X till new sample ready VL6180X.Poll_Range(address); // read range result range = VL6180X.Read_Range(address); // clear the interrupt on VL6180X VL6180X.Clear_Interrupts(address); return range; } int Servo1Pos; int Servo2Pos; /*-------------------------------------------------------------------------------- Function name: KeySub Input Parameters: std_msgs::UInt8 &KeyPress Output Parameters: Description: ----------------------------------------------------------------------------------*/ int main(){ uint8_t TOFRange[4]; while(1){ TOFRange[0] = serviceTOF(ADDR1); TOFRange[1] = serviceTOF(ADDR2); TOFRange[2] = serviceTOF(ADDR3); TOFRange[3] = serviceTOF(ADDR4); //Send range to pc by serial pc.printf("TOF1:%d TOF3: %d TOF5: %d TOF7: %d\r\n", TOFRange[0], TOFRange[1], TOFRange[2],TOFRange[3]); //Short delay wait(0.1); void MotorKeySub(const std_msgs::UInt8 &keyPress) { printf("%d", keyPress.data); // Lowercase chars // if (keyPress.data == 119) { // w if (keyPress.data == 88 || 120) { // X MotorR.Stop(); MotorL.Stop(); } else if(TOFRange[2]<150){ MotorR.Stop(); MotorL.Stop(); } else { MotorR.Forwards(1.0f); MotorL.Forwards(1.0f); wait(0.05); } } else if (keyPress.data == 97) { // a if (keyPress.data == 88 || 120) { // X MotorR.Stop(); MotorL.Stop(); } else if (TOFRange[3]<150){ MotorR.Stop(); MotorL.Stop(); } else{ MotorR.Forwards(1.0f); MotorL.Backwards(1.0f); wait(0.05); } } else if (keyPress.data == 100) { // d if (keyPress.data == 88 || 120) { // X MotorR.Stop(); MotorL.Stop(); } else if(TOFRange[1]<150){ MotorR.Stop(); MotorL.Stop(); } else{ MotorR.Backwards(1.0f); MotorL.Forwards(1.0f); wait(0.05); } } else if (keyPress.data == 115) { // s if (keyPress.data == 88 || 120) { // X MotorR.Stop(); MotorL.Stop(); } else if(TOFRange[0]<150){ MotorR.Stop(); MotorL.Stop(); } else{ MotorR.Backwards(1.0f); MotorL.Backwards(1.0f); wait(0.05); } } // Upper Case Chars else if (keyPress.data == 87) { // W if (keyPress.data == 88 || 120) { // X MotorR.Stop(); MotorL.Stop(); } else if(TOFRange[2]<150){ MotorR.Stop(); MotorL.Stop(); } else{ MotorR.Forwards(1.0f); MotorL.Forwards(1.0f); wait(0.387); } } else if (keyPress.data == 65) { // A if (keyPress.data == 88 || 120) { // X MotorR.Stop(); MotorL.Stop(); } else if (TOFRange[3]<150){ MotorR.Stop(); MotorL.Stop(); } else{ MotorR.Forwards(1.0f); MotorL.Backwards(1.0f); wait(0.387); } } else if (keyPress.data == 68) { // D if (keyPress.data == 88 || 120) { // X MotorR.Stop(); MotorL.Stop(); } else if(TOFRange[1]<150){ MotorR.Stop(); MotorL.Stop(); } else{ MotorR.Backwards(1.0f); MotorL.Forwards(1.0f); wait(0.387); } } else if (keyPress.data == 83) { // S if (keyPress.data == 88 || 120) { // X MotorR.Stop(); MotorL.Stop(); } else if(TOFRange[0]<150){ MotorR.Stop(); MotorL.Stop(); } else{ MotorR.Backwards(1.0f); MotorL.Backwards(1.0f); wait(0.387); } } else { MotorR.Stop(); MotorL.Stop(); } } } } /*-------------------------------------------------------------------------------- Function name: update_power_levels Input Parameters: vBatt Output Parameters: N/A Description: Function used to send the battery level over to the LED class locally from global source files ----------------------------------------------------------------------------------*/ void update_power_levels(float vBatt) { cRGB_LED.record_power(vBatt); } /*-------------------------------------------------------------------------------- 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... cBuzzer.Beep(); cRGB_LED.blue_led(); wait(0.02); cRGB_LED.display_power(); 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) { cBuzzer.Beep(); cRGB_LED.blue_led(); wait(0.02); cRGB_LED.display_power(); 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) { cBuzzer.Beep(); cRGB_LED.blue_led(); wait(0.02); cRGB_LED.display_power(); MotorR.Forwards(0.1f); MotorL.Forwards(0.9f); } else if(TOFRange[0]<200) { cBuzzer.Beep(); cRGB_LED.blue_led(); wait(0.02); cRGB_LED.display_power(); 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; } /*-------------------------------------------------------------------------------- Function name: ServoKeySub Input Parameters: Output Parameters: Description: ----------------------------------------------------------------------------------*/ /* Ascii values for arrow Keys 37(left arrow) 38(up arrow) 39(right arrow) 40(down arrow) */ void servoKeySub(const std_msgs::UInt8 &ServoKeyPress) { printf("%d", ServoKeyPress.data); if (ServoKeyPress.data == 106) { // j for Pan Left if(Servo1Pos>-45) { Servo1Pos = Servo1Pos-IncSize; servo1.position(Servo1Pos); wait(0.01); } } else if (ServoKeyPress.data == 108) { // l for Pan Right if(Servo1Pos<45) { Servo1Pos = Servo1Pos+IncSize; servo1.position(Servo1Pos); wait(0.01); } } else if (ServoKeyPress.data == 105) { // i for Tilt Up if(Servo2Pos>-45){ Servo2Pos = Servo2Pos-IncSize; servo2.position(Servo2Pos); wait(0.01); } } else if (ServoKeyPress.data == 107) { // K for Tilt Down if(Servo2Pos<45){ Servo2Pos = Servo2Pos+IncSize; servo2.position(Servo2Pos); wait(0.01); } } else if (ServoKeyPress.data == 44) { // for Up if(IncSize<20) { IncSize=(IncSize+1); } } else if (ServoKeyPress.data == 46) { // for Up if(IncSize>1) { IncSize=(IncSize-1); } } pc.printf("Servo 1 = %d Servo 2 = %d \n\r",Servo1Pos,Servo2Pos); servo1.position(Servo1Pos); servo2.position(Servo2Pos); }