#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    Gripperpotent,  sensorPotentiometer)
#pragma config(Sensor, dgtl5,  encbl,          sensorQuadEncoder)
#pragma config(Sensor, dgtl7,  encbr,          sensorQuadEncoder)
#pragma config(Sensor, dgtl9,  encfl,          sensorQuadEncoder)
#pragma config(Sensor, dgtl11, encfr,          sensorQuadEncoder)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_3,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_4,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port1,           backLeft,      tmotorVex393HighSpeed_HBridge, PIDControl, reversed, encoderPort, dgtl5)
#pragma config(Motor,  port2,           frontLeft,     tmotorVex393HighSpeed_MC29, PIDControl, encoderPort, dgtl9)
#pragma config(Motor,  port3,           lifttopright,  tmotorVex393HighSpeed_MC29, PIDControl, encoderPort, I2C_2)
#pragma config(Motor,  port4,           liftbottomright, tmotorVex393HighSpeed_MC29, PIDControl, encoderPort, I2C_1)
#pragma config(Motor,  port5,           grippermotor,  tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port7,           lifttopleft,   tmotorVex393HighSpeed_MC29, PIDControl, reversed, encoderPort, I2C_3)
#pragma config(Motor,  port8,           liftbottomleft, tmotorVex393HighSpeed_MC29, PIDControl, reversed, encoderPort, I2C_4)
#pragma config(Motor,  port9,           frontRight,    tmotorVex393HighSpeed_MC29, PIDControl, reversed, encoderPort, dgtl11)
#pragma config(Motor,  port10,          backRight,     tmotorVex393HighSpeed_HBridge, PIDControl, encoderPort, dgtl7)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
   // Variables pour roues
   int X2 = 0, Y1 = 0, X1 = 0, threshold = 15;

   // Variables pour ascenseur et gripper

   int gripperpotclose = 300;
   int gripperpotopen = 2500;



   while(1 == 1)
   {

      // Code pour roues
      if(abs(vexRT[Ch3]) > threshold)
         Y1 = vexRT[Ch3];
      else
         Y1 = 0;
      if(abs(vexRT[Ch4]) > threshold)
         X1 = vexRT[Ch4];
      else
         X1 = 0;
      if(abs(vexRT[Ch1]) > threshold)
         X2 = vexRT[Ch1];
      else
         X2 = 0;

      motor[frontRight] = Y1 - X2 - X1;
      motor[backRight] =  Y1 - X2 + X1;
      motor[frontLeft] = Y1 + X2 + X1;
      motor[backLeft] =  Y1 + X2 - X1;


      resetMotorEncoder(lifttopright);
      resetMotorEncoder(liftbottomright);
      resetMotorEncoder(lifttopleft);
      resetMotorEncoder(liftbottomleft);

      // Code pour lancer l'objet bouton 6U
      if(vexRT[Btn6U] == 1)
      {
         motor(lifttopright) = 127;
         motor(liftbottomright) = 127;
         motor(lifttopleft) = 127;
         motor(liftbottomleft) = 127;

         if(getMotorEncoder(lifttopright)>200)
         {
            if(SensorValue(Gripperpotent) < gripperpotopen)
            {
               motor[grippermotor] = -127;
            }
         }
         else motor[grippermotor] = 0;
      }
      else if(getMotorEncoder(lifttopright)>5)
      {
         motor(lifttopright) = -80;
         motor(liftbottomright) = -80;
         motor(lifttopleft) = -80;
         motor(liftbottomleft) = -80;
      }
      else
      {
         motor(lifttopright) = 0;
         motor(liftbottomright) = 0;
         motor(lifttopleft) = 0;
         motor(liftbottomleft) = 0;
      }

                // Controle manuel du gripper avec les boutons 8D (ferme) et 7D (ouvre)

      if(vexRT(Btn7U)==1) //ferme le gripper
      {
         if(SensorValue(Gripperpotent) > gripperpotclose)
            motor[grippermotor] = 127;
      }
      else
         motor[grippermotor] = 0;

      if(vexRT(Btn7D)==1) //ouvre le gripper
      {
         if(SensorValue(Gripperpotent) < gripperpotopen)
            motor[grippermotor] = -127;
      }
      else
         motor[grippermotor] = 0;

      // Code pour lever l'ascenseur Bouton 5U
      if(vexRT[Btn5U] == 1)
      {
         motor(lifttopright) = 90;
         motor(liftbottomright) = 90;
         motor(lifttopleft) = 90;
         motor(liftbottomleft) = 90;
      }

      if(vexRT[Btn5D] == 1)
      {
         motor(lifttopright) = -75;
         motor(liftbottomright) = -75;
         motor(lifttopleft) = -75;
         motor(liftbottomleft) = -75;
      }

      wait1Msec(20);   // ralentir le cycle à 50 Hz (0,02 secondes par cycle)
   }
}