SOFT564Z Group 3 / Mbed 2 deprecated SOFT564Z_Group_3v3

with the tof code

Dependencies:   mbed Servo ros_lib_kinetic

Motors/Motor.cpp@7:8248af58df5a, 2019-11-19 (annotated)

Committer:
Stumi
Date:
Tue Nov 19 12:55:44 2019 +0000
Revision:
7:8248af58df5a
Parent:
6:2cc2aac35868
Child:
8:262c6c40bff9
Added blue user button function and LED to show power status

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 7:8248af58df5a 6 #include "Motor.h"
Stumi 6:2cc2aac35868 7 #include "LED.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 7:8248af58df5a 11
Stumi 7:8248af58df5a 12 //Class definitions
Stumi 7:8248af58df5a 13 cBuzzer cBuzzer(Buzz);
Stumi 6:2cc2aac35868 14 cRGB_LED cRGB_LED(DIAG_RED, DIAG_BLU, DIAG_GRN);
Stumi 7:8248af58df5a 15
Stumi 7:8248af58df5a 16
Stumi 7:8248af58df5a 17 /*--------------------------------------------------------------------------------
Stumi 7:8248af58df5a 18 Function name: update_power_levels
Stumi 7:8248af58df5a 19 Input Parameters: vBatt
Stumi 7:8248af58df5a 20 Output Parameters: N/A
Stumi 7:8248af58df5a 21 Description: Function used to send the battery level over to the LED class locally from global source files
Stumi 7:8248af58df5a 22 ----------------------------------------------------------------------------------*/
Stumi 7:8248af58df5a 23 void update_power_levels(float vBatt)
Stumi 7:8248af58df5a 24 {
Stumi 7:8248af58df5a 25 cRGB_LED.record_power(vBatt);
Stumi 7:8248af58df5a 26 }
Stumi 4:36a04230554d 27
Stumi 4:36a04230554d 28 /*--------------------------------------------------------------------------------
Stumi 4:36a04230554d 29 Function name: Check_for_obstacles
Stumi 4:36a04230554d 30 Input Parameters: cMotor - MotorL (left motor) cMotor MotorR (right motor) TOFrange[4] - sensor measurements
Stumi 4:36a04230554d 31 Output Parameters: N/A
Stumi 4:36a04230554d 32 Description: Simple obstacle avoidance functionality
Stumi 4:36a04230554d 33 ----------------------------------------------------------------------------------*/
Stumi 4:36a04230554d 34 void Check_for_obstacles(uint8_t TOFRange[4])
Stumi 4:36a04230554d 35 {
Stumi 4:36a04230554d 36 //If top right sensor(6) detects something
Stumi 4:36a04230554d 37 if (TOFRange[2]<200) {
Stumi 7:8248af58df5a 38
Stumi 4:36a04230554d 39 if(TOFRange[2]>150) { //Provided its 15cm away...
Stumi 7:8248af58df5a 40 cBuzzer.Beep();
Stumi 7:8248af58df5a 41 cRGB_LED.blue_led();
Stumi 7:8248af58df5a 42 wait(0.02);
Stumi 7:8248af58df5a 43 cRGB_LED.display_power();
Stumi 4:36a04230554d 44 if(TOFRange[3]<200) { //...and the back sensor detects something
Stumi 4:36a04230554d 45 //Smooth turn right
Stumi 4:36a04230554d 46 MotorR.Forwards(0.8f);
Stumi 4:36a04230554d 47 MotorL.Forwards(0.2f);
Stumi 7:8248af58df5a 48
Stumi 4:36a04230554d 49 } else if(TOFRange[1]<200) { //...and the top left sensor detects something
Stumi 4:36a04230554d 50 //Smooth turn left
Stumi 4:36a04230554d 51 MotorR.Forwards(0.2f);
Stumi 4:36a04230554d 52 MotorL.Forwards(0.8f);
Stumi 7:8248af58df5a 53
Stumi 4:36a04230554d 54 } else {
Stumi 4:36a04230554d 55 MotorR.Forwards(0.8f);
Stumi 4:36a04230554d 56 MotorL.Forwards(0.2f);
Stumi 4:36a04230554d 57 }
Stumi 7:8248af58df5a 58
Stumi 4:36a04230554d 59 } else {
Stumi 4:36a04230554d 60
Stumi 4:36a04230554d 61 if(TOFRange[3]<200) {
Stumi 4:36a04230554d 62 MotorR.Backwards(0.1f);
Stumi 4:36a04230554d 63 MotorL.Backwards(0.9f);
Stumi 7:8248af58df5a 64
Stumi 4:36a04230554d 65 } else if(TOFRange[1]<200) {
Stumi 4:36a04230554d 66 MotorR.Backwards(0.9f);
Stumi 4:36a04230554d 67 MotorL.Backwards(0.1f);
Stumi 7:8248af58df5a 68
Stumi 4:36a04230554d 69 } else {
Stumi 4:36a04230554d 70 MotorR.Backwards(0.1f);
Stumi 4:36a04230554d 71 MotorL.Backwards(0.9f);
Stumi 4:36a04230554d 72 }
Stumi 4:36a04230554d 73 }
Stumi 7:8248af58df5a 74
Stumi 4:36a04230554d 75 } else if(TOFRange[3]<200) {
Stumi 7:8248af58df5a 76 cBuzzer.Beep();
Stumi 7:8248af58df5a 77 cRGB_LED.blue_led();
Stumi 7:8248af58df5a 78 wait(0.02);
Stumi 7:8248af58df5a 79 cRGB_LED.display_power();
Stumi 7:8248af58df5a 80
Stumi 4:36a04230554d 81 if(TOFRange[1]<200) {
Stumi 4:36a04230554d 82 MotorR.Forwards(1.0f);
Stumi 4:36a04230554d 83 MotorL.Forwards(1.0f);
Stumi 4:36a04230554d 84 } else {
Stumi 7:8248af58df5a 85
Stumi 4:36a04230554d 86 MotorR.Forwards(0.9f);
Stumi 4:36a04230554d 87 MotorL.Forwards(0.1f);
Stumi 4:36a04230554d 88 }
Stumi 7:8248af58df5a 89
Stumi 4:36a04230554d 90 } else if(TOFRange[1]<200) {
Stumi 7:8248af58df5a 91 cBuzzer.Beep();
Stumi 7:8248af58df5a 92 cRGB_LED.blue_led();
Stumi 7:8248af58df5a 93 wait(0.02);
Stumi 7:8248af58df5a 94 cRGB_LED.display_power();
Stumi 4:36a04230554d 95 MotorR.Forwards(0.1f);
Stumi 4:36a04230554d 96 MotorL.Forwards(0.9f);
Stumi 7:8248af58df5a 97
Stumi 4:36a04230554d 98 } else if(TOFRange[0]<200) {
Stumi 7:8248af58df5a 99 cBuzzer.Beep();
Stumi 7:8248af58df5a 100 cRGB_LED.blue_led();
Stumi 7:8248af58df5a 101 wait(0.02);
Stumi 7:8248af58df5a 102 cRGB_LED.display_power();
Stumi 4:36a04230554d 103 MotorR.Forwards(1.0f);
Stumi 4:36a04230554d 104 MotorL.Forwards(1.0f);
Stumi 4:36a04230554d 105 }
Stumi 4:36a04230554d 106
Stumi 4:36a04230554d 107 else {
Stumi 4:36a04230554d 108 MotorR.Forwards(1.0f);
Stumi 4:36a04230554d 109 MotorL.Forwards(1.0f);
Stumi 4:36a04230554d 110 }
Stumi 4:36a04230554d 111 }
Stumi 4:36a04230554d 112
Stumi 2:6197e1cf2cd1 113 /*--------------------------------------------------------------------------------
Stumi 2:6197e1cf2cd1 114 Function name: cMotor
Stumi 2:6197e1cf2cd1 115 Input Parameters: PwmOut pwm - Motor PWM, DigitalOut fwd - Motor input1, DigitalOut rev - Motor Input2
Stumi 2:6197e1cf2cd1 116 Output Parameters: N/A
Stumi 2:6197e1cf2cd1 117 Description: Class constructor (Initialisation upon creating class)
Stumi 2:6197e1cf2cd1 118 ----------------------------------------------------------------------------------*/
Stumi 4:36a04230554d 119 cMotor::cMotor(PwmOut pwm, DigitalOut fwd, DigitalOut rev):_pwm(pwm), _fwd(fwd), _rev(rev)
Stumi 4:36a04230554d 120 {
Stumi 4:36a04230554d 121
Stumi 2:6197e1cf2cd1 122 // Set initial condition of PWM
Stumi 2:6197e1cf2cd1 123 _pwm.period(0.001); //1KHz
Stumi 2:6197e1cf2cd1 124 _pwm = 0;
Stumi 2:6197e1cf2cd1 125
Stumi 2:6197e1cf2cd1 126 // Initial condition of output enables
Stumi 2:6197e1cf2cd1 127 _fwd = 0;
Stumi 2:6197e1cf2cd1 128 _rev = 0;
Stumi 2:6197e1cf2cd1 129 }
Stumi 2:6197e1cf2cd1 130
Stumi 2:6197e1cf2cd1 131 /*--------------------------------------------------------------------------------
Stumi 2:6197e1cf2cd1 132 Function name: Forwards
Stumi 2:6197e1cf2cd1 133 Input Parameters: float speed - PWM duty between 0-1
Stumi 2:6197e1cf2cd1 134 Output Parameters: N/A
Stumi 2:6197e1cf2cd1 135 Description: Drives the motor forwards at a designated speed
Stumi 2:6197e1cf2cd1 136 ----------------------------------------------------------------------------------*/
Stumi 4:36a04230554d 137 void cMotor::Forwards(float speed)
Stumi 4:36a04230554d 138 {
Stumi 2:6197e1cf2cd1 139 _fwd = 1;
Stumi 2:6197e1cf2cd1 140 _rev = 0;
Stumi 2:6197e1cf2cd1 141 _pwm = speed;
Stumi 2:6197e1cf2cd1 142 }
Stumi 2:6197e1cf2cd1 143
Stumi 2:6197e1cf2cd1 144 /*--------------------------------------------------------------------------------
Stumi 2:6197e1cf2cd1 145 Function name: Backwards
Stumi 2:6197e1cf2cd1 146 Input Parameters: float speed - PWM duty between 0-1
Stumi 2:6197e1cf2cd1 147 Output Parameters: N/A
Stumi 2:6197e1cf2cd1 148 Description: Drives the motor backwards at a designated speed
Stumi 2:6197e1cf2cd1 149 ----------------------------------------------------------------------------------*/
Stumi 4:36a04230554d 150 void cMotor::Backwards(float speed)
Stumi 4:36a04230554d 151 {
Stumi 2:6197e1cf2cd1 152 _fwd = 0;
Stumi 2:6197e1cf2cd1 153 _rev = 1;
Stumi 2:6197e1cf2cd1 154 _pwm = speed;
Stumi 2:6197e1cf2cd1 155 }
Stumi 3:df6160e2f6d9 156
Stumi 3:df6160e2f6d9 157 /*--------------------------------------------------------------------------------
Stumi 3:df6160e2f6d9 158 Function name: Stop
Stumi 3:df6160e2f6d9 159 Input Parameters: N/A
Stumi 3:df6160e2f6d9 160 Output Parameters: N/A
Stumi 3:df6160e2f6d9 161 Description: Drives the motor backwards at a designated speed
Stumi 3:df6160e2f6d9 162 ----------------------------------------------------------------------------------*/
Stumi 4:36a04230554d 163 void cMotor::Stop()
Stumi 4:36a04230554d 164 {
Stumi 3:df6160e2f6d9 165 _fwd = 0;
Stumi 3:df6160e2f6d9 166 _rev = 0;
Stumi 3:df6160e2f6d9 167 _pwm = 0;
Stumi 3:df6160e2f6d9 168 }