IRsensor.cpp

00001 #include <mbed.h>
00002 #include <math.h>
00003 #include "IRSensor_H.h"
00004 
00005 
00006 IRSensor::IRSensor(PinName pin1,PinName pin2,PinName pin3,PinName pin4,PinName pin5, float Kp, float Ki, float Kd, float noErSpeed, float turnSpeed):
00007     m_leftIR(pin1), m_midLeftIR(pin2), m_midIR(pin3), m_midRightIR(pin4), m_rightIR(pin5), m_Kp(Kp), m_Ki(Ki), m_Kd(Kd){
00008     //class constructor, initialises pins and variables and gets a first reading on the sensors  
00009         
00010     m_P = 0.0;
00011     m_I = 0.0;
00012     m_D = 0.0;
00013     
00014     m_Kp = Kp;
00015     m_Ki = Ki;
00016     m_Kd = Kd;
00017     m_noErSpeed = noErSpeed;
00018     m_turnSpeed = turnSpeed;
00019     
00020     
00021     m_toggle = false;
00022     
00023     m_dirL = true;
00024     m_dirR = true;
00025     
00026     m_prevDirL = true;
00027     m_prevDirR = true;
00028     
00029     m_color = 5;
00030     
00031     Sample();
00032         
00033 }
00034 
00035 
00036 void IRSensor::Sample(){
00037     //function attached to the ticker
00038     //assigns the data recieved for each digital in into an array
00039     //toggle is toggled at every ISR, signifying the ISR has occured
00040     
00041     m_lineSensor[0] = m_leftIR;
00042     m_lineSensor[1] = m_midLeftIR;
00043     m_lineSensor[2] = m_midIR;
00044     m_lineSensor[3] = m_midRightIR;
00045     m_lineSensor[4] = m_rightIR;
00046     
00047     m_toggle = !m_toggle;
00048 }
00049 
00050 
00051 void IRSensor::WeightPID(){
00052     
00053     int i;
00054     int count = 0;
00055     
00056     for(i = 0; i <5; i++){
00057         
00058         
00059         if(!m_lineSensor[i]){
00060             
00061             if(!count){
00062                 m_error = (i-2)*2;
00063                 count++;
00064                 
00065             }else if(count == 1){
00066                 ((m_error >= 0) ^ ((i-2)*2<0))||((m_error <= 0) ^ ((i-2)*2>0)) ? m_error++ : m_error = m_color;
00067                 count++;
00068                 }
00069             else if(count == 2){ 
00070                 ((m_error >= 0) ^ ((i-2)*2<0))?  : m_error = m_color;
00071                 count++;
00072                 }
00073             else if(count>2){ printf("error : to many reading");}
00074             
00075         }
00076     }
00077 
00078     if(!count){m_error = m_prevError;}// m_prevError<0 ? m_error = -5 : m_error = 5;}
00079 }
00080     
00081 
00082 void IRSensor::CalculatePID(){
00083     //as name suggests, calculates the proportions of corrections to be applied to the motors
00084     //error : error given by weightPID
00085     //*previousError : pointer to the previous error calculated by weightPID
00086     //returns the PID value
00087     
00088     m_P = m_error*1.0;
00089     m_I = (m_I + m_error)*1.0;
00090     m_D = 1.0*(m_error - m_prevError);
00091         
00092     m_prevError = m_error;
00093     
00094     m_PID = m_Kp*m_P + m_Ki*m_I + m_Kd*m_D;
00095     //printf("error is : %i , Kp is :  %f , P is : %f   \r",m_error, m_Kp, m_P);    
00096     prevPID = m_PID;
00097 }
00098 
00099 
00100 void IRSensor::MotorControl(){
00101      //assigns the calculated direction to the motors
00102     //PIDvalue : calculated PID value given by CalculatePID
00103     //initSpeed : speed without correction 
00104     //check previousSpeed to make sure the direction is the same
00105     float initSpeed = 0.07;
00106     float noErSpeed = 0.17;
00107     float error;
00108     
00109     
00110     //scale and assign speed
00111     error = (m_PID / 5.0);
00112     //error *= exp(fabs(error)*-1.5)+1;
00113     
00114     if(error == 0){
00115         m_speedL = noErSpeed;
00116         m_speedR = noErSpeed; 
00117         
00118         m_dirL = true;
00119         m_dirR = true;
00120         return;
00121     
00122     }       
00123     
00124     //printf("m_error : %f \t error : %f \r", m_error, error);
00125     m_speedL = initSpeed - error;
00126     m_speedR = initSpeed + error;
00127     
00128     m_speedL >= 0 ? m_dirL = true : m_dirL = false;
00129     //if(m_dirL != m_prevDirL) controlLeft.SetDirection(m_dirL);
00130     
00131     m_speedR >= 0 ? m_dirR = true : m_dirR = false;
00132     //if(m_dirR != m_prevDirR) controlRight.SetDirection(m_dirR);
00133     
00134     printf("leftspeed is :  %f, rightspeed is :  %f \r", m_speedL, m_speedR);
00135     
00136     
00137     
00138 }