Nicolae Marton / Mbed 2 deprecated TDP3test

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

IRSensorPID.cpp@0:9d67126e11c8, 2019-01-25 (annotated)

Committer:
Nicolaemf
Date:
Fri Jan 25 14:09:48 2019 +0000
Revision:
0:9d67126e11c8
Child:
1:d96e4201ff84
motor code implemented with non complete pid controller

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Nicolaemf 0:9d67126e11c8 1 #include <mbed.h>
Nicolaemf 0:9d67126e11c8 2
Nicolaemf 0:9d67126e11c8 3
Nicolaemf 0:9d67126e11c8 4 /* One enable, forward or backwards, controlling both sides
Nicolaemf 0:9d67126e11c8 5 4 PWM lines, two for each side, one controlling forward speed and one for backward speed for EACH side
Nicolaemf 0:9d67126e11c8 6 driving sequence is pwm off, enable toggle, pwm on */
Nicolaemf 0:9d67126e11c8 7
Nicolaemf 0:9d67126e11c8 8 // motor "enable"
Nicolaemf 0:9d67126e11c8 9 //DigitalOut motorEnableLeft(PTC12); // right motor "enable" (1 = forward, 0 = reverse)
Nicolaemf 0:9d67126e11c8 10 //DigitalOut motorEnableRight(PTA13);
Nicolaemf 0:9d67126e11c8 11
Nicolaemf 0:9d67126e11c8 12
Nicolaemf 0:9d67126e11c8 13 // motor PWM controls
Nicolaemf 0:9d67126e11c8 14 // left motors forward speed control (blue wire) -> PTA5
Nicolaemf 0:9d67126e11c8 15 // right motors forward speed control (green wire) -> PTA4
Nicolaemf 0:9d67126e11c8 16 // left motors backward speed control (yellow wire) -> PTD4
Nicolaemf 0:9d67126e11c8 17 // right motors backward speed control (white wire) -> PTA12
Nicolaemf 0:9d67126e11c8 18
Nicolaemf 0:9d67126e11c8 19 //Left Enable (... wire) -> PTC12
Nicolaemf 0:9d67126e11c8 20 //Right Enable (... wire) -> PTA13
Nicolaemf 0:9d67126e11c8 21
Nicolaemf 0:9d67126e11c8 22 // In order to stop, PWM duty cycle must be = 0
Nicolaemf 0:9d67126e11c8 23 // Enable controls direction wheels spin'
Nicolaemf 0:9d67126e11c8 24
Nicolaemf 0:9d67126e11c8 25
Nicolaemf 0:9d67126e11c8 26 typedef struct PwmDir{
Nicolaemf 0:9d67126e11c8 27 //PwmDir contains two PWM, on for the left motor one for the right motor
Nicolaemf 0:9d67126e11c8 28 DigitalOut motorEnable;
Nicolaemf 0:9d67126e11c8 29 PwmOut motorBw;
Nicolaemf 0:9d67126e11c8 30 PwmOut motorFw;
Nicolaemf 0:9d67126e11c8 31
Nicolaemf 0:9d67126e11c8 32 //constructor, when create a struct, need to specify the pin it will take.
Nicolaemf 0:9d67126e11c8 33 //will need to set the pwm value outisde the struct
Nicolaemf 0:9d67126e11c8 34 PwmDir(PinName pin1, PinName pin2, PinName pin3) : motorEnable(pin1), motorBw(pin2), motorFw(pin3) {};
Nicolaemf 0:9d67126e11c8 35
Nicolaemf 0:9d67126e11c8 36 }PwmDir;
Nicolaemf 0:9d67126e11c8 37
Nicolaemf 0:9d67126e11c8 38 //initialize the pwms and their pin numbers here, 1 enable 2 PwmBw 3 PwmFw
Nicolaemf 0:9d67126e11c8 39 PwmDir pwmLeft(PTC12,PTD4,PTA5);
Nicolaemf 0:9d67126e11c8 40 PwmDir pwmRight(PTA13,PTA12,PTA4);
Nicolaemf 0:9d67126e11c8 41
Nicolaemf 0:9d67126e11c8 42
Nicolaemf 0:9d67126e11c8 43 PwmOut* SetDirection(bool direction,bool pwmSelect){
Nicolaemf 0:9d67126e11c8 44 /*set direction of one of the sides depending on pwmSelect
Nicolaemf 0:9d67126e11c8 45 direction : 0 go forward ,1 go backwards
Nicolaemf 0:9d67126e11c8 46 pwmSelect : 0 left motors ,1 right motors */
Nicolaemf 0:9d67126e11c8 47
Nicolaemf 0:9d67126e11c8 48 if(!pwmSelect){
Nicolaemf 0:9d67126e11c8 49 //modify the left motor's direction
Nicolaemf 0:9d67126e11c8 50 //set every pwm to 0
Nicolaemf 0:9d67126e11c8 51 pwmLeft.motorBw.write(0);
Nicolaemf 0:9d67126e11c8 52 pwmLeft.motorFw.write(0);
Nicolaemf 0:9d67126e11c8 53
Nicolaemf 0:9d67126e11c8 54 wait(0.1);
Nicolaemf 0:9d67126e11c8 55
Nicolaemf 0:9d67126e11c8 56 if(!direction){
Nicolaemf 0:9d67126e11c8 57 pwmLeft.motorEnable = false;
Nicolaemf 0:9d67126e11c8 58 return &(pwmLeft.motorFw);
Nicolaemf 0:9d67126e11c8 59 }
Nicolaemf 0:9d67126e11c8 60 else{
Nicolaemf 0:9d67126e11c8 61 pwmLeft.motorEnable = true;
Nicolaemf 0:9d67126e11c8 62 return &(pwmLeft.motorBw);
Nicolaemf 0:9d67126e11c8 63 }
Nicolaemf 0:9d67126e11c8 64 }else{
Nicolaemf 0:9d67126e11c8 65 //modify right motors direction
Nicolaemf 0:9d67126e11c8 66 //set every pwm to 0
Nicolaemf 0:9d67126e11c8 67 pwmRight.motorBw.write(0);
Nicolaemf 0:9d67126e11c8 68 pwmRight.motorFw.write(0);
Nicolaemf 0:9d67126e11c8 69
Nicolaemf 0:9d67126e11c8 70 wait(0.1);
Nicolaemf 0:9d67126e11c8 71
Nicolaemf 0:9d67126e11c8 72 if(!direction){
Nicolaemf 0:9d67126e11c8 73 pwmRight.motorEnable = false;
Nicolaemf 0:9d67126e11c8 74 return &(pwmRight.motorFw);
Nicolaemf 0:9d67126e11c8 75 }
Nicolaemf 0:9d67126e11c8 76 else{
Nicolaemf 0:9d67126e11c8 77 pwmRight.motorEnable = true;
Nicolaemf 0:9d67126e11c8 78 return &(pwmRight.motorBw);
Nicolaemf 0:9d67126e11c8 79 }
Nicolaemf 0:9d67126e11c8 80 }
Nicolaemf 0:9d67126e11c8 81 }
Nicolaemf 0:9d67126e11c8 82
Nicolaemf 0:9d67126e11c8 83 void SetSpeed(float leftSpeed, float rightSpeed, PwmOut *pwmLeftPtr, PwmOut *pwmRightPtr) {
Nicolaemf 0:9d67126e11c8 84 // set speed of either forward or backward PWMs
Nicolaemf 0:9d67126e11c8 85 // set both wheels to same speed
Nicolaemf 0:9d67126e11c8 86 // NOTE: set enable BEFORE calling function
Nicolaemf 0:9d67126e11c8 87 // speed: speed to be set
Nicolaemf 0:9d67126e11c8 88 // mRight & mLeft: pass PwmOut names of relevant Right and Left motor controls
Nicolaemf 0:9d67126e11c8 89
Nicolaemf 0:9d67126e11c8 90 if(speed<=1.0 && speed > 0.0){
Nicolaemf 0:9d67126e11c8 91 pwmLeftPtr->write(speed);
Nicolaemf 0:9d67126e11c8 92 pwmRightPtr->write(speed);
Nicolaemf 0:9d67126e11c8 93 }else if(speed < 0 && speed >= 1.0){
Nicolaemf 0:9d67126e11c8 94 SetDirection(0, 0);
Nicolaemf 0:9d67126e11c8 95 SetDirection(0, 1);
Nicolaemf 0:9d67126e11c8 96 }
Nicolaemf 0:9d67126e11c8 97
Nicolaemf 0:9d67126e11c8 98 }
Nicolaemf 0:9d67126e11c8 99
Nicolaemf 0:9d67126e11c8 100 void SetSpeed(float speed, PwmOut *pwmDirPtr) {
Nicolaemf 0:9d67126e11c8 101 // set speed of passed the pointer to PwmOut
Nicolaemf 0:9d67126e11c8 102 // sets speed to only right or left depending on which pointer is passed
Nicolaemf 0:9d67126e11c8 103
Nicolaemf 0:9d67126e11c8 104 if(speed<=1.0){
Nicolaemf 0:9d67126e11c8 105 (*pwmDirPtr).write(speed);
Nicolaemf 0:9d67126e11c8 106 }
Nicolaemf 0:9d67126e11c8 107 }
Nicolaemf 0:9d67126e11c8 108 //-----------------------------------------------------------
Nicolaemf 0:9d67126e11c8 109
Nicolaemf 0:9d67126e11c8 110
Nicolaemf 0:9d67126e11c8 111
Nicolaemf 0:9d67126e11c8 112 typedef struct prevDirect{
Nicolaemf 0:9d67126e11c8 113 bool prevDirL;
Nicolaemf 0:9d67126e11c8 114 bool prevDirR;
Nicolaemf 0:9d67126e11c8 115 }prevDir;
Nicolaemf 0:9d67126e11c8 116
Nicolaemf 0:9d67126e11c8 117
Nicolaemf 0:9d67126e11c8 118 //sampling is a repeated interrupt that gathers the data from the IRsensors
Nicolaemf 0:9d67126e11c8 119 Ticker sampling;
Nicolaemf 0:9d67126e11c8 120
Nicolaemf 0:9d67126e11c8 121 DigitalIn leftIR(PTC6);
Nicolaemf 0:9d67126e11c8 122 DigitalIn midLeftIR(PTC5);
Nicolaemf 0:9d67126e11c8 123 DigitalIn midIR(PTC4);
Nicolaemf 0:9d67126e11c8 124 DigitalIn midRightIR(PTC3);
Nicolaemf 0:9d67126e11c8 125 DigitalIn rightIR(PTC0);
Nicolaemf 0:9d67126e11c8 126
Nicolaemf 0:9d67126e11c8 127 bool lineSensor[5], toggle2 = false;
Nicolaemf 0:9d67126e11c8 128
Nicolaemf 0:9d67126e11c8 129
Nicolaemf 0:9d67126e11c8 130 void Sample(){
Nicolaemf 0:9d67126e11c8 131 //function attached to the ticker
Nicolaemf 0:9d67126e11c8 132 //assigns the data recieved for each digital in into an array
Nicolaemf 0:9d67126e11c8 133 //toggle is toggled at every ISR, signifying the ISR has occured
Nicolaemf 0:9d67126e11c8 134
Nicolaemf 0:9d67126e11c8 135 lineSensor[0] = leftIR;
Nicolaemf 0:9d67126e11c8 136 lineSensor[1] = midLeftIR;
Nicolaemf 0:9d67126e11c8 137 lineSensor[2] = midIR;
Nicolaemf 0:9d67126e11c8 138 lineSensor[3] = midRightIR;
Nicolaemf 0:9d67126e11c8 139 lineSensor[4] = rightIR;
Nicolaemf 0:9d67126e11c8 140
Nicolaemf 0:9d67126e11c8 141 toggle2 = !toggle2;
Nicolaemf 0:9d67126e11c8 142 }
Nicolaemf 0:9d67126e11c8 143
Nicolaemf 0:9d67126e11c8 144 //algorithm to be done when we have the final version of the hardware
Nicolaemf 0:9d67126e11c8 145 int WeightPID(bool toggle, int error){
Nicolaemf 0:9d67126e11c8 146 //assign the weight of the error depending on which sensor is on
Nicolaemf 0:9d67126e11c8 147 //adds a count to change the weight when two sensors are on to change
Nicolaemf 0:9d67126e11c8 148
Nicolaemf 0:9d67126e11c8 149 int i;
Nicolaemf 0:9d67126e11c8 150 int count = 0;
Nicolaemf 0:9d67126e11c8 151
Nicolaemf 0:9d67126e11c8 152 //if the interrupt has occured, pursue the PID calculations
Nicolaemf 0:9d67126e11c8 153
Nicolaemf 0:9d67126e11c8 154 for(i = 0; i < 5; i++){
Nicolaemf 0:9d67126e11c8 155 if(lineSensor[i] && !count){
Nicolaemf 0:9d67126e11c8 156 count++;
Nicolaemf 0:9d67126e11c8 157 error = 2*(i-2);
Nicolaemf 0:9d67126e11c8 158
Nicolaemf 0:9d67126e11c8 159
Nicolaemf 0:9d67126e11c8 160 } else if(lineSensor[i] && count == 1){
Nicolaemf 0:9d67126e11c8 161 error++;
Nicolaemf 0:9d67126e11c8 162 count++;
Nicolaemf 0:9d67126e11c8 163
Nicolaemf 0:9d67126e11c8 164 }else if(lineSensor[i] && count == 2){
Nicolaemf 0:9d67126e11c8 165 //decide to turn right or left depending on the colour of the disk
Nicolaemf 0:9d67126e11c8 166 //should be defined by a boolean, with 0 equals left and 1 equals right or vice versa
Nicolaemf 0:9d67126e11c8 167 }
Nicolaemf 0:9d67126e11c8 168 }
Nicolaemf 0:9d67126e11c8 169
Nicolaemf 0:9d67126e11c8 170 if(!count){
Nicolaemf 0:9d67126e11c8 171 if(error == 2 || error == -2) error = 0;
Nicolaemf 0:9d67126e11c8 172 else error<0 ? error = -5 : error = 5;
Nicolaemf 0:9d67126e11c8 173 }
Nicolaemf 0:9d67126e11c8 174
Nicolaemf 0:9d67126e11c8 175 return error;
Nicolaemf 0:9d67126e11c8 176 }
Nicolaemf 0:9d67126e11c8 177
Nicolaemf 0:9d67126e11c8 178
Nicolaemf 0:9d67126e11c8 179
Nicolaemf 0:9d67126e11c8 180 float CalculatePID(int error, int *previousError){
Nicolaemf 0:9d67126e11c8 181 //as name suggests, calculates the proportions of corrections to be applied to the motors
Nicolaemf 0:9d67126e11c8 182 //error : error given by weightPID
Nicolaemf 0:9d67126e11c8 183 //*previousError : pointer to the previous error calculated by weightPID
Nicolaemf 0:9d67126e11c8 184 //returns the PID value
Nicolaemf 0:9d67126e11c8 185
Nicolaemf 0:9d67126e11c8 186 float P = 0.0, I = 0.0, D = 0.0;
Nicolaemf 0:9d67126e11c8 187 float Kp = 1.0, Ki = 0.0, Kd = 1.0;
Nicolaemf 0:9d67126e11c8 188
Nicolaemf 0:9d67126e11c8 189 P = error;
Nicolaemf 0:9d67126e11c8 190 I = I + error;
Nicolaemf 0:9d67126e11c8 191 D = error - *previousError;
Nicolaemf 0:9d67126e11c8 192
Nicolaemf 0:9d67126e11c8 193 *previousError = error;
Nicolaemf 0:9d67126e11c8 194
Nicolaemf 0:9d67126e11c8 195 return Kp*P + Ki*I + Kd*D;
Nicolaemf 0:9d67126e11c8 196
Nicolaemf 0:9d67126e11c8 197 }
Nicolaemf 0:9d67126e11c8 198
Nicolaemf 0:9d67126e11c8 199
Nicolaemf 0:9d67126e11c8 200
Nicolaemf 0:9d67126e11c8 201 prevDirect MotorControl(float PIDValue, float initSpeed, prevDirect prevDir){
Nicolaemf 0:9d67126e11c8 202 //assigns the calculated direction to the motors
Nicolaemf 0:9d67126e11c8 203 //PIDvalue : calculated PID value given by CalculatePID
Nicolaemf 0:9d67126e11c8 204 //initSpeed : speed without correction
Nicolaemf 0:9d67126e11c8 205 //check previousSpeed to make sure the direction is the same
Nicolaemf 0:9d67126e11c8 206
Nicolaemf 0:9d67126e11c8 207
Nicolaemf 0:9d67126e11c8 208 float highThresh = 0.2, lowThresh = 0.2;
Nicolaemf 0:9d67126e11c8 209 float maxPID = 5.0, minPID = -5.0;
Nicolaemf 0:9d67126e11c8 210 int dirLeft, dirRight;
Nicolaemf 0:9d67126e11c8 211 bool left
Nicolaemf 0:9d67126e11c8 212
Nicolaemf 0:9d67126e11c8 213 //assign new speed with PID value and scale it
Nicolaemf 0:9d67126e11c8 214 float leftSpeed, rightSpeed;
Nicolaemf 0:9d67126e11c8 215
Nicolaemf 0:9d67126e11c8 216 //scale and assign speed
Nicolaemf 0:9d67126e11c8 217 PIDValue = PIDValue / 5;
Nicolaemf 0:9d67126e11c8 218 leftSpeed = initSpeed - PIDValue;
Nicolaemf 0:9d67126e11c8 219 rightSpeed = initSpeed + PIDValue;
Nicolaemf 0:9d67126e11c8 220
Nicolaemf 0:9d67126e11c8 221 leftSpeed > 0 ? dirLeft = true : dirLeft = false;
Nicolaemf 0:9d67126e11c8 222 if(leftSpeed == prevDir.prevDirL) setDirection(...);
Nicolaemf 0:9d67126e11c8 223
Nicolaemf 0:9d67126e11c8 224 righSpeed > 0 ? dirRight = true : dirRight = false;
Nicolaemf 0:9d67126e11c8 225 if(rightSpeed == prevDir.prevDirR) setDirection(...);
Nicolaemf 0:9d67126e11c8 226
Nicolaemf 0:9d67126e11c8 227 SetSpeed(...)
Nicolaemf 0:9d67126e11c8 228
Nicolaemf 0:9d67126e11c8 229
Nicolaemf 0:9d67126e11c8 230
Nicolaemf 0:9d67126e11c8 231 return prevDir
Nicolaemf 0:9d67126e11c8 232 }
Nicolaemf 0:9d67126e11c8 233
Nicolaemf 0:9d67126e11c8 234 //------------------------------------------------
Nicolaemf 0:9d67126e11c8 235
Nicolaemf 0:9d67126e11c8 236
Nicolaemf 0:9d67126e11c8 237
Nicolaemf 0:9d67126e11c8 238
Nicolaemf 0:9d67126e11c8 239 int main(){
Nicolaemf 0:9d67126e11c8 240
Nicolaemf 0:9d67126e11c8 241 //pointers which point to either PWM forward or backwards of left and right
Nicolaemf 0:9d67126e11c8 242 PwmOut *pwmLeftPtr;
Nicolaemf 0:9d67126e11c8 243 PwmOut *pwmRightPtr;
Nicolaemf 0:9d67126e11c8 244
Nicolaemf 0:9d67126e11c8 245 //Assign pwm forward to the pointers
Nicolaemf 0:9d67126e11c8 246 pwmLeftPtr = &(pwmLeft.motorFw);
Nicolaemf 0:9d67126e11c8 247 pwmRightPtr = &(pwmRight.motorFw);
Nicolaemf 0:9d67126e11c8 248
Nicolaemf 0:9d67126e11c8 249 //give a period of 1khz to every pwm
Nicolaemf 0:9d67126e11c8 250 pwmLeft.motorBw.period_ms(1);
Nicolaemf 0:9d67126e11c8 251 pwmLeft.motorFw.period_ms(1);
Nicolaemf 0:9d67126e11c8 252 pwmRight.motorBw.period_ms(1);
Nicolaemf 0:9d67126e11c8 253 pwmRight.motorFw.period_ms(1);
Nicolaemf 0:9d67126e11c8 254
Nicolaemf 0:9d67126e11c8 255 //duty cycle of 0
Nicolaemf 0:9d67126e11c8 256 pwmLeft.motorBw.write(0);
Nicolaemf 0:9d67126e11c8 257 pwmLeft.motorFw.write(0);
Nicolaemf 0:9d67126e11c8 258 pwmRight.motorBw.write(0);
Nicolaemf 0:9d67126e11c8 259 pwmRight.motorFw.write(0);
Nicolaemf 0:9d67126e11c8 260
Nicolaemf 0:9d67126e11c8 261
Nicolaemf 0:9d67126e11c8 262 float speed = 0.2;
Nicolaemf 0:9d67126e11c8 263 //float angle = 1.0;
Nicolaemf 0:9d67126e11c8 264 bool directionLeft = 0;
Nicolaemf 0:9d67126e11c8 265 bool directionRight = 0;
Nicolaemf 0:9d67126e11c8 266
Nicolaemf 0:9d67126e11c8 267 //first set direction left and right to forward
Nicolaemf 0:9d67126e11c8 268 pwmLeftPtr = SetDirection(directionLeft, 0);
Nicolaemf 0:9d67126e11c8 269 pwmRightPtr = SetDirection(directionRight, 1);
Nicolaemf 0:9d67126e11c8 270
Nicolaemf 0:9d67126e11c8 271
Nicolaemf 0:9d67126e11c8 272
Nicolaemf 0:9d67126e11c8 273
Nicolaemf 0:9d67126e11c8 274
Nicolaemf 0:9d67126e11c8 275 short int error = 0, previousError, i;
Nicolaemf 0:9d67126e11c8 276 int *previousErrorPtr;
Nicolaemf 0:9d67126e11c8 277 float PIDValue = 0, initSpeed = 0.3, prevSpeedL, prevSpeedR;
Nicolaemf 0:9d67126e11c8 278 bool toggle = false;
Nicolaemf 0:9d67126e11c8 279
Nicolaemf 0:9d67126e11c8 280 prevDir.prevDirL = directionLeft;
Nicolaemf 0:9d67126e11c8 281 prevDir.prevDirR = directionRight;
Nicolaemf 0:9d67126e11c8 282
Nicolaemf 0:9d67126e11c8 283 previousErrorPtr = &previousError;
Nicolaemf 0:9d67126e11c8 284
Nicolaemf 0:9d67126e11c8 285 //initialize sensor array with 0s
Nicolaemf 0:9d67126e11c8 286 for(i = 0; i<5; i++) lineSensor[i] = 0;
Nicolaemf 0:9d67126e11c8 287
Nicolaemf 0:9d67126e11c8 288 //get a first reading
Nicolaemf 0:9d67126e11c8 289 Sample();
Nicolaemf 0:9d67126e11c8 290 sampling.attach(&Sample, 0.01);
Nicolaemf 0:9d67126e11c8 291
Nicolaemf 0:9d67126e11c8 292 prevSpeedL = initSpeed;
Nicolaemf 0:9d67126e11c8 293 prevSpeedR = initSpeed;
Nicolaemf 0:9d67126e11c8 294
Nicolaemf 0:9d67126e11c8 295 while(1){
Nicolaemf 0:9d67126e11c8 296
Nicolaemf 0:9d67126e11c8 297 //check if interrupt has occured
Nicolaemf 0:9d67126e11c8 298 if(toggle != toggle2){
Nicolaemf 0:9d67126e11c8 299 error = WeightPID(toggle, error);
Nicolaemf 0:9d67126e11c8 300 PIDValue = CalculatePID(error, previousErrorPtr);
Nicolaemf 0:9d67126e11c8 301 MotorControl(PIDValue, initSpeed, prevSpeedLPtr, prevSpeedRPtr);
Nicolaemf 0:9d67126e11c8 302 toggle = toggle2;
Nicolaemf 0:9d67126e11c8 303 }
Nicolaemf 0:9d67126e11c8 304
Nicolaemf 0:9d67126e11c8 305
Nicolaemf 0:9d67126e11c8 306 }
Nicolaemf 0:9d67126e11c8 307
Nicolaemf 0:9d67126e11c8 308
Nicolaemf 0:9d67126e11c8 309 }