SOFT564Z Group 3 / Mbed 2 deprecated SOFT564Z_Group_3v3

with the tof code

Dependencies:   mbed Servo ros_lib_kinetic

Motors/Motor.cpp@4:36a04230554d, 2019-11-07 (annotated)

Committer:
Stumi
Date:
Thu Nov 07 15:31:52 2019 +0000
Revision:
4:36a04230554d
Parent:
3:df6160e2f6d9
Child:
6:2cc2aac35868
Added power management class and obstacle avoidance

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