/*read IR sensors*/
void readSensor(void)
{
u32 curt;
//read DC value
LFSensor = read_LF_Sensor;
RFSensor = read_RF_Sensor;
DLSensor = read_DL_Sensor;
DRSensor = read_DR_Sensor;
curt = micros();
//left front sensor
LF_EM_ON;
elapseMicros(60,curt);
LFSensor = read_LF_Sensor - LFSensor;
LF_EM_OFF;
if(LFSensor < 0)//error check
LFSensor = 0;
elapseMicros(140,curt);
//right front sensor
RF_EM_ON;
elapseMicros(200,curt);
RFSensor = read_RF_Sensor - RFSensor;
RF_EM_OFF;
if(RFSensor < 0)
RFSensor = 0;
elapseMicros(280,curt);
//diagonal sensors
SIDE_EM_ON;
elapseMicros(340,curt);
DLSensor = read_DL_Sensor - DLSensor;
DRSensor = read_DR_Sensor - DRSensor;
SIDE_EM_OFF;
if(DLSensor < 0)
DLSensor = 0;
if(DRSensor < 0)
DRSensor = 0;
readVolMeter();
LFSensor = LFSensor*reflectionRate/1000;
RFSensor = RFSensor*reflectionRate/1000;
DLSensor = DLSensor*reflectionRate/1000;
DRSensor = DRSensor*reflectionRate/1000;
//delay_us(80);
//elapseMicros(500,curt);
}
/**
* Hug Front Wall
*/
void hugFrontWall(int LSensorVal, int RSensorVal) {
while (1) {
int curt = micros(); //start to track time in order to make one adjust every 1000us
readSensor();
setLeftPwm(LSensorVal - LFSensor);
setRightPwm(RSensorVal - RFSensor);
elapseMicros(1000, curt); //elapse 1000 micro seconds
}
}
