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?
User | Revision | Line number | New 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 | } |