/*

Author: Thijs Lacquet and Joris Dalderup, Team: NL0032 (Hermes)

Created on: 20-12-2013

This script now consists of ALL TeleOp code needed to complete
a full game. However, optimization is still a necessity to make
sure that
a) All code is human-readeble (Thijs' optimal Dutch is NOT human-
readable and shoud be replaced by GBE)

b) The NXT and RobotC do not encounter any issues compiling and
running the code an compilations of it.

Note to Stijn: if, when compiling this code, you encounter any
problems, befriend j._dalderup on skype.
*/


#pragma config(Hubs, S1, MatrxRbtcs, none, none, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Motor, motorA, , tmotorNXT, openLoop)
#pragma config(Motor, motorB, , tmotorNXT, openLoop)
#pragma config(Motor, motorC, , tmotorNXT, openLoop)
#pragma config(Motor, mtr_Matrix_S1_1, LeftMotor, tmotorMatrix, openLo
op)
#pragma config(Motor, mtr_Matrix_S1_2, RightMotor, tmotorMatrix, openLo
op, reversed)
#pragma config(Motor, mtr_Matrix_S1_3, LiftMotor_1, tmotorMatrix, openL
oop)
#pragma config(Motor, mtr_Matrix_S1_4, LiftMotor_2, tmotorMatrix, openL
oop)
#pragma config(Servo, srvo_Matrix_S1_1, PlatformServo, tServoStanda
rd)
#pragma config(Servo, srvo_Matrix_S1_2, servo2, tServoNone)
#pragma config(Servo, srvo_Matrix_S1_3, servo3, tServoNone)
#pragma config(Servo, srvo_Matrix_S1_4, servo4, tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard
!!*//



#pragma DebuggerWindows("joystickGame")

#include "JoystickDriver.c"

//Calibration config:
static short THRESHOLD = 5; //Minimum value for controller-axis to be active.

static int SERVOSTATE_HORIZONTALPLATFORM = 0; //Range 0 - 255, servo value of th
e platformservo when in horizontal mode
static int SERVOSTATE_DUMPPLATFORM= 100; //Range 0 - 255, servo value of the pla
tformservo when in dump mode

static int LIFTMOTOR_SPEED = 100; //Range: [0, 1]

static float DRIVESPEED_NORMAL = 1; //Range: [0, 1]
static float DRIVESPEED_SLOW = 0.3; //Range: [0, 1]

static int PLATFORM_SERVOSPEED = 0.2; //Range: [-100, 100], gets multiplied to t
he platformjoystick from the "gunner"
static int MIN_PLATFORMSERVO = 0; //Range: [0, 255], min value of the platformse
rvo when the "gunner" controls it
static int MAX_PLATFORMSERVO = 255; //Rage: [0, 255], max value of the platform
servo when the "gunner" controls it

static int PLATFORMSPEED_LIFT_LOWERS = 5; //Range: [0, ?>, speed of the platform
while the lift lowers, this is controlled by the driver

static int LIFTANDPLATFORM_UPDATE_SPEED = 100; //Range: [0, ?>, time in ms betwe
en updates of the LiftAndPlatform task,
// changing this value also chan
ges the speed of PlatformServo when manually controlled by the "Gunner"

//Button config:
static int SLOWDRIVE_LEFT_BTN = 5;
static int SLOWDRIVE_RIGHT_BTN = 6;
static int LIFT_RAISE_BTN = 0;
static int LIFT_LOWER_BTN = 0;
static int PLATFORM_DUMP_BTN = 0;
static int HANGING_ENABLE_BTN = 0; //R1 btn from second joystick

//End of config

bool hang = false;

void initializeRobot() //This gets executed before the start of the match, so se
rvo's can be sinitialized to starting postition
{
servo[PlatformServo] = SERVOSTATE_HORIZONTALPLATFORM;
return;
}

float DevideJoystick(float joy) //Reduces output from the Joystick to a number b
etween -100 and 100
{
if (joy > 0)
{
return (joy / 1.27);
}
else
{
return (joy / 1.28);
}
}

task drive //Makes it possible to drive using the 2 y-axis of the gamepad with t
ankdrive
{
while (true)
{
getJoystickSettings(joystick); //Updates the joysticks

if (joy1Btn(SLOWDRIVE_LEFT_BTN)) //Left button
{
motor[LeftMotor] = DevideJoystick(joystick.joy1_y1)*DRIVESPEED_SLOW;
}
else
{
motor[LeftMotor] = DevideJoystick(joystick.joy1_y1)*DRIVESPEED_NORMAL;
}

if (joy1Btn(SLOWDRIVE_RIGHT_BTN)) //Right button
{
motor[RightMotor] = DevideJoystick(joystick.joy1_y2)*DRIVESPEED_SLOW;
}
else
{
motor[RightMotor] = DevideJoystick(joystick.joy1_y2)*DRIVESPEED_NORMAL;
}
}
}

task liftAndPlatform
{
while (true)
{
wait1Msec(LIFTANDPLATFORM_UPDATESPEED); //Waits 0,1s before buttons get updated

//Hang:
if (joy2Btn(HANGING_ENABLE_BTN)) //R1 button of 2nd joystick
{
hang = true; //Disables control of lift and platformservo for the drive
r

motor[LiftMotor_1] = DevideJoystick(joystick.joy2_y1);
motor[LiftMotor_2] = DevideJoystick(joystick.joy2_y1);

if (DevideJoystick(joystick.joy2_y2) < -THRESHOLD || DevideJoystick(joy
stick.joy2_y2) > THRESHOLD) //Make it trip over threshold
{
if (ServoValue(PlatformServo) > MIN_PLATFORMSERVO && ServoValue(Platfor
mServo) < MAX_PLATFORMSERVO) //Limits the rotation of the servo
{
servo(PlatformServo) = ServoValue(PlatformServo) + joystick.joy2_y2*PLA
TFORM_SERVOSPEED;
}
}
}
else
{
//Control platform:
if (joy1Btn(PLATFORM_RAISE_BTN) && !joy1Btn(PLATFORM_LOWER_BTN))
{
motor[LiftMotor_1] = LIFTMOTOR_SPEED; //Raises platform
motor[LiftMotor_2] = LIFTMOTOR_SPEED;
}
else
{
if (joy1Btn(PLATFORM_LOWER_BTN) && !joy1Btn(PLATFORM_RAISE_BTN))
{
motor[LiftMotor_1] = -LIFTMOTOR_SPEED; //Lowers platform
motor[LiftMotor_2] = -LIFTMOTOR_SPEED;

if (ServoValue[PlatformServo] < SERVOSTATE_HORIZONTALSERVO)
{
servo[PlatformServo] = ServoValue[PlatformServo] - PLATFORMSPEED_LIFT_L
OWERS; //Makes sure platform stays horizontal while lowering.
}
else
{
motor[LiftMotor_1] = 0; //Stops the platform from moving
motor[LiftMotor_2] = 0;
}
}
}

if (joy1Btn(PLATFORM_DUMP_BTN))
{
servo[PlatformServo] = SERVOSTATE_DUMPPLATFORM;
}
}
}
}
}

task main()
{
initializeRobot(); //Task doe things like setting initial servo values.

waitForStart(); //Waits for the start of the TeleOp phase.

StartTask(drive);
StartTask(liftAndPlatform);

bool noError = true; //Besides being useful as tool to crash the program, thi
s also prevents some weird error from showing up.
while (noError == true)
{

}
}