#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  none)
#pragma config(Motor,  motorA,          ,              tmotorNormal, PIDControl, )
#pragma config(Motor,  motorB,          ,              tmotorNormal, PIDControl, )
#pragma config(Motor,  motorD,          leftMotor,     tmotorNormal, openLoop, )
#pragma config(Motor,  motorE,          rightMotor,    tmotorNormal, openLoop, reversed)
#pragma config(Servo,  servo1,          eServoleft,           tServoNormal)
#pragma config(Servo,  servo2,          eServoright,          tServoNormal)
#pragma config(Servo,  servo3,          bServoleft,           tServoNormal)
#pragma config(Servo,  servo4,          bServoright,          tServoNormal)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*//you should replace all of what we already have with this but just comment it out we might still need it
#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  none)

#pragma config(Motor,  mtr_S1_C1_1,     leftMotor,        tmotorNormal, openLoop,)
#pragma config(Motor,  mtr_S1_C1_2,     rightMotor,        tmotorNormal, openLoop,)
#pragma config(Motor,  mtr_S1_C3_2,     basMo,     tmotorNormal, openLoop,)
#pragma config(motor,  mtr_S1_C3_1
#pragma config(Servo,  servo1,          armServo,             tServoNorma)
#pragma config(Servo,  servo2,          elbowServo,             tServoNormal)

//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*/ //you might need to redo this^
//you will need to figure out the stuff for the autonomous, sorry
#include "JoystickDriver.c"


void endServoControler();
void motorControl();
void elbowControl();
//void autonomousMode();

task main()
{
   while (true)
   {
      while (true)
      {
         getJoystickSettings(joystick);
         while (/*joystick.UserMode =*/ true) // look it up in help it should tell you exactly what variable to use
        {
        getJoystickSettings(joystick);
           elbowControl(); //the one at the "elbow" of the robot
           motorControl();
           endServoControler();
           elbowControl();
         }
      /*while (joystick.UserMode = false)
      {
         autonomousMode(); // you might need to put a getJoystickSettings(joystick) or something before this, check
      }*/
     }
   }
}

void elbowControl()//motorBotor at the base that swings it up or down
{
   int basePower = 0;

   if (joy1Btn(2))
      basePower = basePower - 75;  // make tower go one way; up i think

   if (joy1Btn(3))
      basePower = basePower + 75;  // Make tower go the other way; down i think

   // pressing both 2 & 3 is the same as pressing none at all

   motor[motorA] = basePower;
   motor[motorB] = basePower;
}

void motorControl()
{
   motor[leftMotor] = joystick.joy1_y1;
   motor[rightMotor] = joystick.joy1_y2;
}

void endServoControler() //servo that does the dumping
{
    if (joy1Btn(1) !=0 && joy1Btn(4) != 0)
   {
      return;
   }
   else
   {
      if (joy1Btn(1) != 0)
      {
         servoTarget[eServoright] = 255;
         servoTarget[eServoleft] = 0;
      }
      if (joy1Btn(4) != 0)
      {
         servoTarget[eServoright] = 0;
         servoTarget[eServoleft] = 255;
      }
   }
}


/*void autonomousMode()
{
  motor[leftMotor] = 100;
  motor[rightMotor] = -100;
}*/