SOFT564Z Group 3 / Mbed 2 deprecated SOFT564Z_Group_3v3

with the tof code

Dependencies:   mbed Servo ros_lib_kinetic

Motors/Motor.cpp@6:2cc2aac35868, 2019-11-12 (annotated)

Committer:
Stumi
Date:
Tue Nov 12 12:56:02 2019 +0000
Revision:
6:2cc2aac35868
Parent:
4:36a04230554d
Child:
7:8248af58df5a
Added buzzer functionality; Added buzzer and LED into obstacle avoidance routine; Made class declaration for led and buzzer in motor.cpp NOT MAIN

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Stumi 2:6197e1cf2cd1 1 /*--------------------------------------------------------------------------------
Stumi 2:6197e1cf2cd1 2 Filename: main.cpp
Stumi 2:6197e1cf2cd1 3 Description: Main program loop
Stumi 2:6197e1cf2cd1 4 --------------------------------------------------------------------------------*/
Stumi 6:2cc2aac35868 5 #include "Buzzer.h"
Stumi 6:2cc2aac35868 6 #include "LED.h"
Stumi 2:6197e1cf2cd1 7 #include "Motor.h"
Stumi 2:6197e1cf2cd1 8
Stumi 4:36a04230554d 9 cMotor MotorL(M1_PWM, M1_IN1, M1_IN2); //Left motor class and pin initialisation
Stumi 4:36a04230554d 10 cMotor MotorR(M2_PWM, M2_IN1, M2_IN2); //Right motor class and pin initialisation
Stumi 6:2cc2aac35868 11 cRGB_LED cRGB_LED(DIAG_RED, DIAG_BLU, DIAG_GRN);
Stumi 6:2cc2aac35868 12 cBuzzer cBuzzer(Buzz);
Stumi 4:36a04230554d 13
Stumi 4:36a04230554d 14 /*--------------------------------------------------------------------------------
Stumi 4:36a04230554d 15 Function name: Check_for_obstacles
Stumi 4:36a04230554d 16 Input Parameters: cMotor - MotorL (left motor) cMotor MotorR (right motor) TOFrange[4] - sensor measurements
Stumi 4:36a04230554d 17 Output Parameters: N/A
Stumi 4:36a04230554d 18 Description: Simple obstacle avoidance functionality
Stumi 4:36a04230554d 19 ----------------------------------------------------------------------------------*/
Stumi 4:36a04230554d 20 void Check_for_obstacles(uint8_t TOFRange[4])
Stumi 4:36a04230554d 21 {
Stumi 4:36a04230554d 22 //If top right sensor(6) detects something
Stumi 4:36a04230554d 23 if (TOFRange[2]<200) {
Stumi 6:2cc2aac35868 24 cBuzzer.Beep();
Stumi 6:2cc2aac35868 25 cRGB_LED.red_led();
Stumi 4:36a04230554d 26 if(TOFRange[2]>150) { //Provided its 15cm away...
Stumi 4:36a04230554d 27
Stumi 4:36a04230554d 28 if(TOFRange[3]<200) { //...and the back sensor detects something
Stumi 4:36a04230554d 29 //Smooth turn right
Stumi 4:36a04230554d 30 MotorR.Forwards(0.8f);
Stumi 4:36a04230554d 31 MotorL.Forwards(0.2f);
Stumi 4:36a04230554d 32
Stumi 4:36a04230554d 33 } else if(TOFRange[1]<200) { //...and the top left sensor detects something
Stumi 4:36a04230554d 34 //Smooth turn left
Stumi 4:36a04230554d 35 MotorR.Forwards(0.2f);
Stumi 4:36a04230554d 36 MotorL.Forwards(0.8f);
Stumi 4:36a04230554d 37
Stumi 4:36a04230554d 38 } else {
Stumi 4:36a04230554d 39 MotorR.Forwards(0.8f);
Stumi 4:36a04230554d 40 MotorL.Forwards(0.2f);
Stumi 4:36a04230554d 41 }
Stumi 4:36a04230554d 42
Stumi 4:36a04230554d 43 } else {
Stumi 4:36a04230554d 44
Stumi 4:36a04230554d 45 if(TOFRange[3]<200) {
Stumi 4:36a04230554d 46 MotorR.Backwards(0.1f);
Stumi 4:36a04230554d 47 MotorL.Backwards(0.9f);
Stumi 4:36a04230554d 48
Stumi 4:36a04230554d 49 } else if(TOFRange[1]<200) {
Stumi 4:36a04230554d 50 MotorR.Backwards(0.9f);
Stumi 4:36a04230554d 51 MotorL.Backwards(0.1f);
Stumi 4:36a04230554d 52
Stumi 4:36a04230554d 53 } else {
Stumi 4:36a04230554d 54 MotorR.Backwards(0.1f);
Stumi 4:36a04230554d 55 MotorL.Backwards(0.9f);
Stumi 4:36a04230554d 56 }
Stumi 4:36a04230554d 57 }
Stumi 4:36a04230554d 58
Stumi 4:36a04230554d 59 } else if(TOFRange[3]<200) {
Stumi 6:2cc2aac35868 60 cBuzzer.Beep();
Stumi 6:2cc2aac35868 61 cRGB_LED.red_led();
Stumi 4:36a04230554d 62 if(TOFRange[1]<200) {
Stumi 4:36a04230554d 63 MotorR.Forwards(1.0f);
Stumi 4:36a04230554d 64 MotorL.Forwards(1.0f);
Stumi 4:36a04230554d 65 } else {
Stumi 4:36a04230554d 66
Stumi 4:36a04230554d 67 MotorR.Forwards(0.9f);
Stumi 4:36a04230554d 68 MotorL.Forwards(0.1f);
Stumi 4:36a04230554d 69 }
Stumi 4:36a04230554d 70
Stumi 4:36a04230554d 71 } else if(TOFRange[1]<200) {
Stumi 6:2cc2aac35868 72 cBuzzer.Beep();
Stumi 6:2cc2aac35868 73 cRGB_LED.red_led();
Stumi 4:36a04230554d 74 MotorR.Forwards(0.1f);
Stumi 4:36a04230554d 75 MotorL.Forwards(0.9f);
Stumi 4:36a04230554d 76
Stumi 4:36a04230554d 77 } else if(TOFRange[0]<200) {
Stumi 6:2cc2aac35868 78 cBuzzer.Beep();
Stumi 6:2cc2aac35868 79 cRGB_LED.red_led();
Stumi 4:36a04230554d 80 MotorR.Forwards(1.0f);
Stumi 4:36a04230554d 81 MotorL.Forwards(1.0f);
Stumi 4:36a04230554d 82 }
Stumi 4:36a04230554d 83
Stumi 4:36a04230554d 84 else {
Stumi 6:2cc2aac35868 85 cRGB_LED.green_led();
Stumi 4:36a04230554d 86 MotorR.Forwards(1.0f);
Stumi 4:36a04230554d 87 MotorL.Forwards(1.0f);
Stumi 4:36a04230554d 88 }
Stumi 4:36a04230554d 89 }
Stumi 4:36a04230554d 90
Stumi 2:6197e1cf2cd1 91 /*--------------------------------------------------------------------------------
Stumi 2:6197e1cf2cd1 92 Function name: cMotor
Stumi 2:6197e1cf2cd1 93 Input Parameters: PwmOut pwm - Motor PWM, DigitalOut fwd - Motor input1, DigitalOut rev - Motor Input2
Stumi 2:6197e1cf2cd1 94 Output Parameters: N/A
Stumi 2:6197e1cf2cd1 95 Description: Class constructor (Initialisation upon creating class)
Stumi 2:6197e1cf2cd1 96 ----------------------------------------------------------------------------------*/
Stumi 4:36a04230554d 97 cMotor::cMotor(PwmOut pwm, DigitalOut fwd, DigitalOut rev):_pwm(pwm), _fwd(fwd), _rev(rev)
Stumi 4:36a04230554d 98 {
Stumi 4:36a04230554d 99
Stumi 2:6197e1cf2cd1 100 // Set initial condition of PWM
Stumi 2:6197e1cf2cd1 101 _pwm.period(0.001); //1KHz
Stumi 2:6197e1cf2cd1 102 _pwm = 0;
Stumi 2:6197e1cf2cd1 103
Stumi 2:6197e1cf2cd1 104 // Initial condition of output enables
Stumi 2:6197e1cf2cd1 105 _fwd = 0;
Stumi 2:6197e1cf2cd1 106 _rev = 0;
Stumi 2:6197e1cf2cd1 107 }
Stumi 2:6197e1cf2cd1 108
Stumi 2:6197e1cf2cd1 109 /*--------------------------------------------------------------------------------
Stumi 2:6197e1cf2cd1 110 Function name: Forwards
Stumi 2:6197e1cf2cd1 111 Input Parameters: float speed - PWM duty between 0-1
Stumi 2:6197e1cf2cd1 112 Output Parameters: N/A
Stumi 2:6197e1cf2cd1 113 Description: Drives the motor forwards at a designated speed
Stumi 2:6197e1cf2cd1 114 ----------------------------------------------------------------------------------*/
Stumi 4:36a04230554d 115 void cMotor::Forwards(float speed)
Stumi 4:36a04230554d 116 {
Stumi 2:6197e1cf2cd1 117 _fwd = 1;
Stumi 2:6197e1cf2cd1 118 _rev = 0;
Stumi 2:6197e1cf2cd1 119 _pwm = speed;
Stumi 2:6197e1cf2cd1 120 }
Stumi 2:6197e1cf2cd1 121
Stumi 2:6197e1cf2cd1 122 /*--------------------------------------------------------------------------------
Stumi 2:6197e1cf2cd1 123 Function name: Backwards
Stumi 2:6197e1cf2cd1 124 Input Parameters: float speed - PWM duty between 0-1
Stumi 2:6197e1cf2cd1 125 Output Parameters: N/A
Stumi 2:6197e1cf2cd1 126 Description: Drives the motor backwards at a designated speed
Stumi 2:6197e1cf2cd1 127 ----------------------------------------------------------------------------------*/
Stumi 4:36a04230554d 128 void cMotor::Backwards(float speed)
Stumi 4:36a04230554d 129 {
Stumi 2:6197e1cf2cd1 130 _fwd = 0;
Stumi 2:6197e1cf2cd1 131 _rev = 1;
Stumi 2:6197e1cf2cd1 132 _pwm = speed;
Stumi 2:6197e1cf2cd1 133 }
Stumi 3:df6160e2f6d9 134
Stumi 3:df6160e2f6d9 135 /*--------------------------------------------------------------------------------
Stumi 3:df6160e2f6d9 136 Function name: Stop
Stumi 3:df6160e2f6d9 137 Input Parameters: N/A
Stumi 3:df6160e2f6d9 138 Output Parameters: N/A
Stumi 3:df6160e2f6d9 139 Description: Drives the motor backwards at a designated speed
Stumi 3:df6160e2f6d9 140 ----------------------------------------------------------------------------------*/
Stumi 4:36a04230554d 141 void cMotor::Stop()
Stumi 4:36a04230554d 142 {
Stumi 3:df6160e2f6d9 143 _fwd = 0;
Stumi 3:df6160e2f6d9 144 _rev = 0;
Stumi 3:df6160e2f6d9 145 _pwm = 0;
Stumi 3:df6160e2f6d9 146 }