Diff: IRSensorPID.cpp

Revision:

0:9d67126e11c8

Child:

1:d96e4201ff84

diff -r 000000000000 -r 9d67126e11c8 IRSensorPID.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IRSensorPID.cpp	Fri Jan 25 14:09:48 2019 +0000
@@ -0,0 +1,309 @@
+#include <mbed.h>
+
+
+/* One enable, forward or backwards, controlling both sides
+4 PWM lines, two for each side, one controlling forward speed and one for backward speed for EACH side
+driving sequence is pwm off, enable toggle, pwm on */
+
+// motor "enable"
+//DigitalOut motorEnableLeft(PTC12); // right motor "enable" (1 = forward, 0 = reverse)
+//DigitalOut motorEnableRight(PTA13);
+
+
+// motor PWM controls
+// left motors forward speed control (blue wire) -> PTA5
+// right motors forward speed control (green wire) -> PTA4
+// left motors backward speed control (yellow wire) -> PTD4
+// right motors backward speed control (white wire) -> PTA12
+
+//Left Enable (... wire) -> PTC12
+//Right Enable (... wire) -> PTA13
+
+// In order to stop, PWM duty cycle must be = 0
+// Enable controls direction wheels spin'
+
+
+typedef struct PwmDir{
+    //PwmDir contains two PWM, on for the left motor one for the right motor
+    DigitalOut motorEnable;
+    PwmOut motorBw;
+    PwmOut motorFw;
+    
+    //constructor, when create a struct, need to specify the pin it will take.
+    //will need to set the pwm value outisde the struct
+    PwmDir(PinName pin1, PinName pin2, PinName pin3) : motorEnable(pin1), motorBw(pin2), motorFw(pin3) {};
+    
+    }PwmDir;
+
+//initialize the pwms and their pin numbers here, 1 enable 2 PwmBw 3 PwmFw
+    PwmDir pwmLeft(PTC12,PTD4,PTA5);
+    PwmDir pwmRight(PTA13,PTA12,PTA4);
+    
+    
+PwmOut* SetDirection(bool direction,bool pwmSelect){
+    /*set direction of one of the sides depending on pwmSelect
+    direction : 0 go forward  ,1 go backwards
+    pwmSelect : 0 left motors ,1 right motors */    
+    
+    if(!pwmSelect){
+    //modify the left motor's direction
+    //set every pwm to 0
+    pwmLeft.motorBw.write(0);
+    pwmLeft.motorFw.write(0);
+    
+    wait(0.1);
+        
+        if(!direction){
+            pwmLeft.motorEnable = false;
+            return &(pwmLeft.motorFw);
+            }
+        else{
+            pwmLeft.motorEnable = true;
+            return &(pwmLeft.motorBw);
+            }  
+    }else{
+    //modify right motors direction
+    //set every pwm to 0
+    pwmRight.motorBw.write(0);
+    pwmRight.motorFw.write(0);
+    
+    wait(0.1);
+    
+        if(!direction){
+            pwmRight.motorEnable = false;
+            return &(pwmRight.motorFw);
+            }
+        else{
+            pwmRight.motorEnable = true;
+            return &(pwmRight.motorBw);
+            }
+        }
+    }
+
+void SetSpeed(float leftSpeed, float rightSpeed, PwmOut *pwmLeftPtr, PwmOut *pwmRightPtr) {
+    // set speed of either forward or backward PWMs
+    // set both wheels to same speed
+    // NOTE: set enable BEFORE calling function
+    // speed: speed to be set
+    // mRight & mLeft: pass PwmOut names of relevant Right and Left motor controls
+    
+    if(speed<=1.0 && speed > 0.0){
+        pwmLeftPtr->write(speed);
+        pwmRightPtr->write(speed);
+    }else if(speed < 0 && speed >= 1.0){
+        SetDirection(0, 0);
+        SetDirection(0, 1);   
+    }
+    
+}
+
+void SetSpeed(float speed, PwmOut *pwmDirPtr) {
+    // set speed of passed the pointer to PwmOut  
+    // sets speed to only right or left depending on which pointer is passed
+    
+    if(speed<=1.0){
+        (*pwmDirPtr).write(speed);
+    }
+}
+//-----------------------------------------------------------
+    
+
+    
+typedef struct prevDirect{
+    bool prevDirL;
+    bool prevDirR; 
+}prevDir;
+
+
+//sampling is a repeated interrupt that gathers the data from the IRsensors
+Ticker sampling;
+
+DigitalIn leftIR(PTC6);
+DigitalIn midLeftIR(PTC5);
+DigitalIn midIR(PTC4);
+DigitalIn midRightIR(PTC3);
+DigitalIn rightIR(PTC0);
+
+bool lineSensor[5], toggle2 = false;
+
+
+void Sample(){
+    //function attached to the ticker
+    //assigns the data recieved for each digital in into an array
+    //toggle is toggled at every ISR, signifying the ISR has occured
+    
+    lineSensor[0] = leftIR;
+    lineSensor[1] = midLeftIR;
+    lineSensor[2] = midIR;
+    lineSensor[3] = midRightIR;
+    lineSensor[4] = rightIR;
+    
+    toggle2 = !toggle2;
+}
+
+//algorithm to be done when we have the final version of the hardware
+int WeightPID(bool toggle, int error){
+    //assign the weight of the error depending on which sensor is on
+    //adds a count to change the weight when two sensors are on to change 
+    
+    int i;
+    int count = 0;
+    
+    //if the interrupt has occured, pursue the PID calculations
+        
+    for(i = 0; i < 5; i++){
+        if(lineSensor[i] && !count){
+            count++;
+            error = 2*(i-2);
+                
+                    
+        } else if(lineSensor[i] && count == 1){
+            error++;
+            count++;
+                    
+        }else if(lineSensor[i] && count == 2){
+            //decide to turn right or left depending on the colour of the disk
+            //should be defined by a boolean, with 0 equals left and 1 equals right or vice versa                
+        }
+    }    
+    
+    if(!count){
+        if(error == 2 || error == -2) error = 0;
+        else error<0 ? error = -5 : error = 5;
+        }  
+        
+    return  error; 
+}
+
+
+
+float CalculatePID(int error, int *previousError){
+    //as name suggests, calculates the proportions of corrections to be applied to the motors
+    //error : error given by weightPID
+    //*previousError : pointer to the previous error calculated by weightPID
+    //returns the PID value
+    
+    float P = 0.0, I = 0.0, D = 0.0;
+    float Kp = 1.0, Ki = 0.0, Kd = 1.0;
+    
+    P = error;
+    I = I + error;
+    D = error - *previousError;
+        
+    *previousError = error;
+    
+    return Kp*P + Ki*I + Kd*D;
+    
+}
+
+
+
+prevDirect MotorControl(float PIDValue, float initSpeed, prevDirect prevDir){
+    //assigns the calculated direction to the motors
+    //PIDvalue : calculated PID value given by CalculatePID
+    //initSpeed : speed without correction 
+    //check previousSpeed to make sure the direction is the same
+    
+    
+    float highThresh = 0.2, lowThresh = 0.2;
+    float maxPID = 5.0, minPID = -5.0;
+    int dirLeft, dirRight;
+    bool left
+    
+    //assign new speed with PID value and scale it
+    float leftSpeed, rightSpeed;
+    
+    //scale and assign speed
+    PIDValue = PIDValue / 5;
+    leftSpeed = initSpeed - PIDValue;
+    rightSpeed = initSpeed + PIDValue;
+    
+    leftSpeed > 0 ? dirLeft = true : dirLeft = false;
+    if(leftSpeed == prevDir.prevDirL) setDirection(...);
+    
+    righSpeed > 0 ? dirRight = true : dirRight = false;
+    if(rightSpeed == prevDir.prevDirR) setDirection(...);
+    
+    SetSpeed(...)
+    
+    
+    
+    return prevDir
+}
+
+//------------------------------------------------
+
+
+
+
+int main(){
+    
+    //pointers which point to either PWM forward or backwards of left and right
+    PwmOut *pwmLeftPtr;
+    PwmOut *pwmRightPtr;
+    
+    //Assign pwm forward to the pointers
+    pwmLeftPtr = &(pwmLeft.motorFw);
+    pwmRightPtr = &(pwmRight.motorFw);
+    
+    //give a period of 1khz to every pwm
+    pwmLeft.motorBw.period_ms(1);
+    pwmLeft.motorFw.period_ms(1);
+    pwmRight.motorBw.period_ms(1);
+    pwmRight.motorFw.period_ms(1);
+    
+    //duty cycle of 0
+    pwmLeft.motorBw.write(0);
+    pwmLeft.motorFw.write(0);
+    pwmRight.motorBw.write(0);
+    pwmRight.motorFw.write(0);
+    
+    
+    float speed = 0.2;
+    //float angle = 1.0;
+    bool directionLeft = 0;
+    bool directionRight = 0;
+
+    //first set direction left and right to forward 
+    pwmLeftPtr = SetDirection(directionLeft, 0);
+    pwmRightPtr = SetDirection(directionRight, 1);
+    
+    
+    
+    
+    
+    short int error = 0, previousError, i;
+    int *previousErrorPtr;
+    float PIDValue = 0, initSpeed = 0.3, prevSpeedL, prevSpeedR;
+    bool toggle = false;
+    
+    prevDir.prevDirL = directionLeft;
+    prevDir.prevDirR = directionRight;
+        
+    previousErrorPtr = &previousError;
+    
+    //initialize sensor array with 0s
+    for(i = 0; i<5; i++) lineSensor[i] = 0;
+    
+    //get a first reading
+    Sample();
+    sampling.attach(&Sample, 0.01);
+    
+    prevSpeedL = initSpeed;
+    prevSpeedR = initSpeed;
+    
+    while(1){
+    
+    //check if interrupt has occured
+        if(toggle != toggle2){
+            error = WeightPID(toggle, error);
+            PIDValue = CalculatePID(error, previousErrorPtr);
+            MotorControl(PIDValue, initSpeed, prevSpeedLPtr, prevSpeedRPtr);
+            toggle = toggle2;
+        }
+    
+        
+    }        
+        
+    
+}
\ No newline at end of file