#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S2,     irSeeker,              sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
#include "drivers/hitechnic-irseeker-v2.h"
#define IRSEEKER_KP      10
#define FORWARD_POWER    50
#define BOUND(n)  (((n) < -100)? -100: ((n) > 100)? 100: (n))
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
    bMotorReflected[motorD] = true;
   bMotorReflected[motorE] = false;

int prevDir = HTIRS2readACDir(irSeeker);
while (true)
{
    int currDir = HTIRS2readACDir(irSeeker);
    if (currDir == 0)
    {

        currDir = prevDir;
    }


    int turnPower = (5 - currDir)*IRSEEKER_KP;
    motor[motorE] = BOUND(FORWARD_POWER + turnPower);
    motor[motorD] = BOUND(FORWARD_POWER + turnPower);
    prevDir = currDir;

    wait1Msec(100);
}
}