File content as of revision 1:a2ceed49374e:

#include <mbed.h>
#include "IRSensor_H.h"
#include "RobotControl_H.h"

IRSensor::IRSensor(PinName pin1,PinName pin2,PinName pin3,PinName pin4,PinName pin5, float Kp, float Ki, float Kd):
    m_leftIR(pin1), m_midLeftIR(pin2), m_midIR(pin3), m_midRightIR(pin4), m_rightIR(pin5), m_Kp(Kp), m_Ki(Ki), m_Kd(Kd){
    //class constructor       
    m_P = 0.0;
    m_I = 0.0;
    m_D = 0.0;
    m_toggle = false;
    Sample();
        
}


void IRSensor::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
    
    m_lineSensor[0] = m_leftIR;
    m_lineSensor[1] = m_midLeftIR;
    m_lineSensor[2] = m_midIR;
    m_lineSensor[3] = m_midRightIR;
    m_lineSensor[4] = m_rightIR;
    
    m_toggle = m_toggle;
}

void IRSensor::WeightPID(){
    
}
    

void IRSensor::CalculatePID(){
    //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
    
    m_P = m_error;
    m_I = m_I + m_error;
    m_D = m_error - m_prevError;
        
    m_prevError = m_error;
    
    m_error = m_Kp*m_P + m_Ki*m_I + m_Kd*m_D;
}


void IRSensor::MotorControl(RobotControl controlLeft, RobotControl controlRight){
     //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 initSpeed = 0.2;
    
    
    //scale and assign speed
    m_error = m_error / 5;
    m_speedL = initSpeed - m_error;
    m_speedR = initSpeed + m_error;
    
    m_speedL > 0 ? m_dirL = true : m_dirR = false;
    if(m_dirL != m_prevDirL) controlLeft.SetDirection(m_dirL);
    
    m_speedR > 0 ? m_dirL = true : m_dirR = false;
    if(m_dirR != m_prevDirR) controlRight.SetDirection(m_dirR);
    
    controlLeft.SetSpeed(m_speedL);
    controlRight.SetSpeed(m_speedR);
    
    m_prevDirL = m_dirL;
    m_prevDirR = m_dirR;
    
    
    
}