File content as of revision 4:645b5d648c64:

#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, initialises pins and variables and gets a first reading on the sensors  
        
    m_P = 0.0;
    m_I = 0.0;
    m_D = 0.0;
    
    m_toggle = false;
    
    m_dirL = true;
    m_dirR = true;
    
    m_prevDirL = true;
    m_prevDirR = true;
    
    m_color = 5;
    
    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(){
    
    int i;
    int count = 0;
    
    for(i = 0; i <5; i++){
        
        if(m_lineSensor[i]){
            if(!count){
                m_error = (i-2)*2;
                count++;
            }else if(count == 1){((m_error >= 0) ^ ((i-2)*2<0))? m_error++ : m_error = m_color;}
            else if(count == 2){ ((m_error >= 0) ^ ((i-2)*2<0))?  : m_error = m_color;}
            else if(count<2){ printf("error : to many reading");}
        }
    }
        
    if(!count){ m_prevError<0 ? m_error = -5 : m_error = 5;}

    m_error = m_prevError;
}
    

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;
    float error;
    
    //scale and assign speed
    error = m_error / 5.0f;
    m_speedL = initSpeed - error;
    m_speedR = initSpeed + 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(abs(m_speedL));
    //controlRight.SetSpeed(abs(m_speedR));
    
    //m_prevDirL = m_dirL;
    //m_prevDirR = m_dirR;
    
    //printf("leftSpeed : %f, rightSpeed %f", m_speedL, m_speedR);
    
    
    
}