Twan Vogelzang / motorDriverRobotWaitress

Deze moet je hebben Menner Dennis

motorDriver.cpp@0:7866cf54b606, 2018-12-19 (annotated)

Committer:
Twan123
Date:
Wed Dec 19 13:23:08 2018 +0000
Revision:
0:7866cf54b606
Motordriver Emily the robotwaitress V1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Twan123 0:7866cf54b606 1 /* This code is written for EMILY the robot Waitress */
Twan123 0:7866cf54b606 2 #include "motorDriver.h"
Twan123 0:7866cf54b606 3 /*============================================================================*/
Twan123 0:7866cf54b606 4 // acceleration speed in ms
Twan123 0:7866cf54b606 5 int accSpeed = 500;
Twan123 0:7866cf54b606 6 int BatteryPubTime =2000;
Twan123 0:7866cf54b606 7
Twan123 0:7866cf54b606 8
Twan123 0:7866cf54b606 9 /*============================================================================*/
Twan123 0:7866cf54b606 10
Twan123 0:7866cf54b606 11
Twan123 0:7866cf54b606 12 motorDriver::motorDriver(PinName Left1, PinName Left2, PinName Right1, PinName Right2, PinName PWML, PinName PWMR /*, PinName spdReadL, PinName spdReadR, PinName Bat*/):
Twan123 0:7866cf54b606 13 speedLeft1(Left1), speedLeft2(Left2), speedRight1(Right1), speedRight2(Right2) , pwmL(PWML), pwmR(PWMR)/*, speedReadL(spdReadL), speedReadR(spdReadR), BatteryRead(Bat)*/
Twan123 0:7866cf54b606 14 {
Twan123 0:7866cf54b606 15 //initial conditions of PWM left and right
Twan123 0:7866cf54b606 16 pwmL.period_ms(10);
Twan123 0:7866cf54b606 17 pwmR.period_ms(10);
Twan123 0:7866cf54b606 18
Twan123 0:7866cf54b606 19 //initial conditions of the left motor
Twan123 0:7866cf54b606 20 speedLeft1= 0;
Twan123 0:7866cf54b606 21 speedLeft2= 0;
Twan123 0:7866cf54b606 22
Twan123 0:7866cf54b606 23 //initial conditions of the right motor
Twan123 0:7866cf54b606 24 speedRight1=0;
Twan123 0:7866cf54b606 25 speedRight2=0;
Twan123 0:7866cf54b606 26
Twan123 0:7866cf54b606 27 }
Twan123 0:7866cf54b606 28
Twan123 0:7866cf54b606 29
Twan123 0:7866cf54b606 30
Twan123 0:7866cf54b606 31
Twan123 0:7866cf54b606 32 void motorDriver::setDirection(bool keyForward, bool keyBackward, bool keyLeft, bool keyRight)
Twan123 0:7866cf54b606 33 {
Twan123 0:7866cf54b606 34 if(keyForward && !keyBackward && !keyLeft && !keyRight) // forward
Twan123 0:7866cf54b606 35 {
Twan123 0:7866cf54b606 36 speedLeft2 = 0;
Twan123 0:7866cf54b606 37 speedRight2 = 0;
Twan123 0:7866cf54b606 38
Twan123 0:7866cf54b606 39 if(speedLeft1 < 1 && speedRight1 <1)
Twan123 0:7866cf54b606 40 {
Twan123 0:7866cf54b606 41 pwmL.pulsewidth_ms(0);
Twan123 0:7866cf54b606 42 pwmR.pulsewidth_ms(0);
Twan123 0:7866cf54b606 43 speedRight1 = 1;
Twan123 0:7866cf54b606 44 speedLeft1 = 1;
Twan123 0:7866cf54b606 45
Twan123 0:7866cf54b606 46 for(int i=0; i<=10; i++)
Twan123 0:7866cf54b606 47 {
Twan123 0:7866cf54b606 48 pwmL.pulsewidth_ms(i);
Twan123 0:7866cf54b606 49 pwmR.pulsewidth_ms(i);
Twan123 0:7866cf54b606 50 Thread::wait(accSpeed);
Twan123 0:7866cf54b606 51 }
Twan123 0:7866cf54b606 52
Twan123 0:7866cf54b606 53 }
Twan123 0:7866cf54b606 54
Twan123 0:7866cf54b606 55 if(speedLeft1 && speedRight1)
Twan123 0:7866cf54b606 56 {
Twan123 0:7866cf54b606 57 speedLeft1 = 1;
Twan123 0:7866cf54b606 58 speedRight1 = 1;
Twan123 0:7866cf54b606 59
Twan123 0:7866cf54b606 60 }
Twan123 0:7866cf54b606 61 }
Twan123 0:7866cf54b606 62
Twan123 0:7866cf54b606 63
Twan123 0:7866cf54b606 64 if(!keyForward && keyBackward && !keyLeft && !keyRight) // Backwards
Twan123 0:7866cf54b606 65 {
Twan123 0:7866cf54b606 66 speedLeft1 = 0;
Twan123 0:7866cf54b606 67 speedRight1 = 0;
Twan123 0:7866cf54b606 68
Twan123 0:7866cf54b606 69 if(speedLeft2 < 1 && speedRight2 <1)
Twan123 0:7866cf54b606 70 {
Twan123 0:7866cf54b606 71 pwmL.pulsewidth_ms(0);
Twan123 0:7866cf54b606 72 pwmR.pulsewidth_ms(0);
Twan123 0:7866cf54b606 73 speedRight2 = 1;
Twan123 0:7866cf54b606 74 speedLeft2 = 1;
Twan123 0:7866cf54b606 75
Twan123 0:7866cf54b606 76 for(int i=0; i<=10; i++)
Twan123 0:7866cf54b606 77 {
Twan123 0:7866cf54b606 78 pwmL.pulsewidth_ms(i);
Twan123 0:7866cf54b606 79 pwmR.pulsewidth_ms(i);
Twan123 0:7866cf54b606 80 Thread::wait(accSpeed);
Twan123 0:7866cf54b606 81 }
Twan123 0:7866cf54b606 82
Twan123 0:7866cf54b606 83 }
Twan123 0:7866cf54b606 84
Twan123 0:7866cf54b606 85 if(speedLeft2 && speedRight2)
Twan123 0:7866cf54b606 86 {
Twan123 0:7866cf54b606 87 speedLeft1 = 1;
Twan123 0:7866cf54b606 88 speedRight1 = 1;
Twan123 0:7866cf54b606 89 Thread::wait(accSpeed);
Twan123 0:7866cf54b606 90 }
Twan123 0:7866cf54b606 91
Twan123 0:7866cf54b606 92 }
Twan123 0:7866cf54b606 93
Twan123 0:7866cf54b606 94 if(!keyForward && !keyBackward && keyLeft && !keyRight) // Left
Twan123 0:7866cf54b606 95 {
Twan123 0:7866cf54b606 96 speedLeft1 = 0;
Twan123 0:7866cf54b606 97 speedLeft2 = 0;
Twan123 0:7866cf54b606 98 speedRight2 = 0;
Twan123 0:7866cf54b606 99
Twan123 0:7866cf54b606 100 if(speedRight1 <1)
Twan123 0:7866cf54b606 101 {
Twan123 0:7866cf54b606 102 pwmR.pulsewidth_ms(0);
Twan123 0:7866cf54b606 103 speedRight1 = 1;
Twan123 0:7866cf54b606 104
Twan123 0:7866cf54b606 105 for(int i=0; i<=10; i++)
Twan123 0:7866cf54b606 106 {
Twan123 0:7866cf54b606 107 pwmR.pulsewidth_ms(i);
Twan123 0:7866cf54b606 108 Thread::wait(accSpeed);
Twan123 0:7866cf54b606 109 }
Twan123 0:7866cf54b606 110
Twan123 0:7866cf54b606 111 }
Twan123 0:7866cf54b606 112
Twan123 0:7866cf54b606 113 if(speedRight1)
Twan123 0:7866cf54b606 114 {
Twan123 0:7866cf54b606 115 speedRight1 = 1;
Twan123 0:7866cf54b606 116 Thread::wait(accSpeed);
Twan123 0:7866cf54b606 117 }
Twan123 0:7866cf54b606 118 }
Twan123 0:7866cf54b606 119
Twan123 0:7866cf54b606 120 if(!keyForward && !keyBackward && !keyLeft && keyRight) // Right
Twan123 0:7866cf54b606 121 {
Twan123 0:7866cf54b606 122 speedRight1 = 0;
Twan123 0:7866cf54b606 123 speedRight2 = 0;
Twan123 0:7866cf54b606 124 speedLeft2 = 0;
Twan123 0:7866cf54b606 125
Twan123 0:7866cf54b606 126 if(speedLeft1 <1)
Twan123 0:7866cf54b606 127 {
Twan123 0:7866cf54b606 128 pwmL.pulsewidth_ms(0);
Twan123 0:7866cf54b606 129 speedLeft1=1;
Twan123 0:7866cf54b606 130
Twan123 0:7866cf54b606 131 for(int i=0; i<=10; i++)
Twan123 0:7866cf54b606 132 {
Twan123 0:7866cf54b606 133 pwmL.pulsewidth_ms(i);
Twan123 0:7866cf54b606 134 Thread::wait(accSpeed);
Twan123 0:7866cf54b606 135 }
Twan123 0:7866cf54b606 136
Twan123 0:7866cf54b606 137 }
Twan123 0:7866cf54b606 138
Twan123 0:7866cf54b606 139 if(speedLeft1)
Twan123 0:7866cf54b606 140 {
Twan123 0:7866cf54b606 141 speedLeft1 = 1;
Twan123 0:7866cf54b606 142 Thread::wait(accSpeed);
Twan123 0:7866cf54b606 143 }
Twan123 0:7866cf54b606 144 }
Twan123 0:7866cf54b606 145 }
Twan123 0:7866cf54b606 146
Twan123 0:7866cf54b606 147
Twan123 0:7866cf54b606 148 //
Twan123 0:7866cf54b606 149 //
Twan123 0:7866cf54b606 150 //
Twan123 0:7866cf54b606 151 //void motorDriver::getBattery()
Twan123 0:7866cf54b606 152 //{
Twan123 0:7866cf54b606 153 //
Twan123 0:7866cf54b606 154 // ros::Publisher pub_battery("BatteryVoltage", &value_msg);
Twan123 0:7866cf54b606 155 // nh.initNode();
Twan123 0:7866cf54b606 156 // nh.advertise(pub_battery);
Twan123 0:7866cf54b606 157 //
Twan123 0:7866cf54b606 158 // float BatteryValue = 0;
Twan123 0:7866cf54b606 159 // float avg_value = 0;
Twan123 0:7866cf54b606 160 //
Twan123 0:7866cf54b606 161 // while(1)
Twan123 0:7866cf54b606 162 // {
Twan123 0:7866cf54b606 163 // for( int i = 0; i < 10; i++)
Twan123 0:7866cf54b606 164 // {
Twan123 0:7866cf54b606 165 // avg_value += BatteryRead; // uitlezen spanning
Twan123 0:7866cf54b606 166 // }
Twan123 0:7866cf54b606 167 //
Twan123 0:7866cf54b606 168 // BatteryValue = (avg_value / 10); // delen door 10 want 10 samples
Twan123 0:7866cf54b606 169 // BatteryValue = BatteryValue * 3.3; // *3.3 Vref
Twan123 0:7866cf54b606 170 // value_msg.data = BatteryValue;
Twan123 0:7866cf54b606 171 // pub_battery.publish(&value_msg);
Twan123 0:7866cf54b606 172 // Thread::wait(BatteryPubTime);
Twan123 0:7866cf54b606 173 // nh.spinOnce();
Twan123 0:7866cf54b606 174 // break;
Twan123 0:7866cf54b606 175 //
Twan123 0:7866cf54b606 176 // }
Twan123 0:7866cf54b606 177 //}