#pragma config(Hubs,  S1, MatrxRbtcs, none,     none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_Matrix_S1_1, LinksAchter,     tmotorMatrix, openLoop)
#pragma config(Motor,  mtr_Matrix_S1_2, RechtsAchter,    tmotorMatrix, openLoop)
#pragma config(Motor,  mtr_Matrix_S1_3, LinksVoor,       tmotorMatrix, openLoop)
#pragma config(Motor,  mtr_Matrix_S1_4, RechtsVoor,      tmotorMatrix, openLoop)
#pragma config(Servo,  srvo_Matrix_S1_1, ArmSchaar1,            tServoStandard)
#pragma config(Servo,  srvo_Matrix_S1_2, ArmSchaar2,            tServoStandard)
#pragma config(Servo,  srvo_Matrix_S1_3, servo3,               tServoNone)
#pragma config(Servo,  srvo_Matrix_S1_4, servo4,               tServoNone)

#include "JoystickDriver.c"                                                //Include the Joystick/Game variables driver

task TTankDrive
{
   int threshold = 6;                                      //Makes it so it dooesn't make unnessesary movs
   while(true)
   {
      getJoystickSettings(joystick);                                  //Updates the game and joystick variables
      if(abs(joystick.joy1_y2) > threshold)
      {
         motor[RechtsVoor] = (-joystick.joy1_y2/2);
      motor[RechtsAchter] = (-joystick.joy1_y2/2);
    }
      else
    {
      motor[RechtsVoor] = 0;
      motor[RechtsAchter] = 0;
    }
    if(abs(joystick.joy1_y1) > threshold)
    {
      motor[LinksVoor] = (joystick.joy1_y1/2);
      motor[LinksAchter] = (joystick.joy1_y1/2);
    }
    else
    {
      motor[LinksVoor] = 0;
      motor[LinksAchter] = 0;
    }
  }
  return;
}

task TSideDrive
{
   int threshold = 6;
   while(true)
   {
      getJoystickSettings(joystick);                                  //Updates the game and joystick variables
      if(abs(joystick.joy1_x2) > threshold)
      {
         motor[RechtsVoor] = (-joystick.joy1_x2/2);
      motor[RechtsAchter] = (-joystick.joy1_x2/2);
    }
      else
    {
      motor[RechtsVoor] = 0;
      motor[RechtsAchter] = 0;
    }
    if(abs(joystick.joy1_x1) > threshold)
    {
      motor[LinksVoor] = (joystick.joy1_x1/2);
      motor[LinksAchter] = (joystick.joy1_x1/2);
    }
    else
    {
      motor[LinksVoor] = 0;
      motor[LinksAchter] = 0;
    }
  }
  return;
}

task TButton
{
   while(1 == 1)
      {
            getJoystickSettings(joystick);
            servoChangeRate[ArmSchaar1] = 3;
            servoChangeRate[ArmSchaar2] = 3;
          if(joy1Btn(08) == 1)
             {
              servo[ArmSchaar1] = 255;
              servo[ArmSchaar2] = 0;
             }
            if (joy1Btn(07) == 1)
               {
                servo[ArmSchaar1] = 0;
                servo[ArmSchaar2] = 255;
          }
        if (joy1Btn(05) == 0)                         //If the LB-button isn't pressed you can drive in Normal mode
               {
                StartTask(TTankDrive);
                StopTask(TSideDrive);
          }
        if (joy1Btn(05) == 1)
               {
                StartTask(TSideDrive);
                StopTask(TTankDrive);
          }
   }
   return;
}

void initializeRobot()
{
   servo[ArmSchaar1] = 0;
   servo[ArmSchaar2] = 0;
   return;
}

task main()
{
  initializeRobot();
  waitForStart();
  motor[LinksVoor] = 20;    motor[RechtsVoor] = -20;
  motor[LinksAchter] = 20;  motor[RechtsAchter] = -20;
  wait1Msec(2000);
  motor[LinksVoor] = 20;    motor[RechtsVoor] = 20;
  motor[LinksAchter] = 20;  motor[RechtsAchter] = 20;
  wait1Msec(4000);
  motor[LinksVoor] = 20;    motor[RechtsVoor] = -20;
  motor[LinksAchter] = 20;  motor[RechtsAchter] = -20;
  wait1Msec(2000);
  motor[LinksVoor] = 0;       motor[RechtsVoor] = 0;
  motor[LinksAchter] = 0;   motor[RechtsAchter] = 0;
   StartTask(TButton);
   while(true)
   {
   }
return;
}