01-  #pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
02-  #pragma config(Sensor, S1,     ,               sensorI2CMuxController)
03-  #pragma config(Motor,  mtr_S1_C1_1,     rightM,        tmotorTetrix, openLoop)
04-  #pragma config(Motor,  mtr_S1_C1_2,     leftM,         tmotorTetrix, openLoop)
05-  #pragma config(Motor,  mtr_S1_C2_1,     armM,          tmotorTetrix, openLoop)
06-  #pragma config(Motor,  mtr_S1_C2_2,     baseM,         tmotorTetrix, openLoop)
07-  #pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoStandard)
08-  #pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoStandard)
09-  #pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
10-  #pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
11-  #pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
12-  #pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
13-  #include "JoystickDriver.c"                         //brings information from the joystickdriver.c file
14-  void InitializeRobot(){
15-     return;
16-  }
17-  task main(){
18-     InitializeRobot();
19-     int threshold = 5;                             //speed threshold
20-     while (1  == 1){                                //infinite loop
21-       getJoystickSettings(joystick);       //gets position of joysticks
22-       int x = joystick.joy1_x1;                //binding the joystick x and y values to variable
23-       int y = joystick.joy1_y1;
24-       int speed = sqrt((pow(x,2)) + (pow(y,2)));   //Pythagorean theorem.
25-       int angle = atan((abs(y) / abs(x))); //finds angle using absolute value of x and y
26-                }
27-      
28-      if (speed > threshold){                   
29-          if (y > 0){
30-          motor(leftM) = (speed *(1 - (angle / 180))); //setting up the speeds multiplied
31-          motor(rightM) = (speed *(angle / 180));      // by the percentage of degree to
32-          }else{                                                                           //180 degrees (theta over 180)
33-             if (y < 0){
34-                  motor(leftM) = ((speed * (1 - (angle / 180))) * -1)); //making values reverse
35-                  motor(rightM) = ((speed * (anlgle / 180)) * -1);
36-               }
37-              }
38-          if (x > -2) && (x < 2)){              // when neither one of the others are true
39-                motor(rightM) = speed;   //it falls back to this statement, where it runs both
40-                motor(rightM) = speed;  //of the motors equally if x  is between -2 and 2
41-             }
42-          }
43-       }

if (condition1)
{
   while (condition2)
   {
      if (condition3)
      {
         doStuff();
      }
      else
      {
         doOtherStuff();
      }
   }
}

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     rightM,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     leftM,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     armM,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     baseM,         tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)

#include "JoystickDriver.c"

void InitializeRobot()
{
   return;
}

task main()
{
   InitializeRobot();
   int threshold = 5;
   while (true)
   {
      getJoystickSettings(joystick);       //gets position of joysticks
      int x = joystick.joy1_x1;                //binding the joystick x and y values to variable
      int y = joystick.joy1_y1;
      int speed = sqrt((pow(x,2)) + (pow(y,2)));   //Pythagorean theorem.
      int angle = atan((abs(y) / abs(x))); //finds angle using absolute value of x and y

      if (speed > threshold)
      {
         if (y > 0)
         {
            motor[leftM] = (speed *(1 - (angle / 180))); //setting up the speeds multiplied
            motor[rightM] = (speed *(angle / 180));      // by the percentage of degree to
         }
         else if (y < 0)
         {
            motor[leftM] = ((speed * (1 - (angle / 180))) * -1); //making values reverse
            motor[rightM] = ((speed * (angle / 180)) * -1);
         }
         if (abs(x) < 2) // when neither one of the others are true
         {             
            motor[rightM] = speed;   //it falls back to this statement, where it runs both
            motor[leftM] = speed;  //of the motors equally if x  is between -2 and 2
         }
      }
   }
}