File content as of revision 5:bb0bec710c91:

#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 <= 0) ^ ((i-2)*2>0)) ? m_error++ : m_error = m_color;
                count++;
                }
            else if(count == 2){ 
                ((m_error >= 0) ^ ((i-2)*2<0))?  : m_error = m_color;
                count++;
                }
            else if(count>2){ printf("error : to many reading");}
            
        }
    }

    if(!count){ m_prevError<0 ? m_error = -5 : m_error = 5;}
}
    

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*1.0;
    m_I = (m_I + m_error)*1.0;
    m_D = 1.0*(m_error - m_prevError);
        
    m_prevError = m_error;
    
    m_PID = m_Kp*m_P + m_Ki*m_I + m_Kd*m_D;
    //printf("error is : %i , Kp is :  %f , P is : %f   \r",m_error, m_Kp, m_P);    

}


void IRSensor::MotorControl(){
     //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.21;
    float error;
    
    //scale and assign speed
    error = m_PID / 5.0;
    
    //printf("m_error : %f \t error : %f \r", m_error, error);
    m_speedL = initSpeed - error;
    m_speedR = initSpeed + error;
    
    m_speedL >= 0 ? m_dirL = true : m_dirL = false;
    //if(m_dirL != m_prevDirL) controlLeft.SetDirection(m_dirL);
    
    m_speedR >= 0 ? m_dirR = true : m_dirR = false;
    //if(m_dirR != m_prevDirR) controlRight.SetDirection(m_dirR);
    
    printf("leftspeed is :  %f, rightspeed is :  %f \r", m_speedL, m_speedR);
    
    
}