curval.c

#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     proto,          sensorI2CCustom9V)
#pragma config(Sensor, S3,     gyro,           sensorI2CHiTechnicGyro)
#pragma config(Motor,  mtr_S1_C1_1,     pod2Drive,     tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     pod3Drive,     tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     pod1Drive,     tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     pod0Drive,     tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    pod0Steer,            tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    pod1Steer,            tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    pod2Steer,            tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    pod3Steer,            tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
#include "drivers/HTSPB-driver.h"
#include "JoystickDriver.c"

float atan2(float x, float y)
{
  float a;
  if (x>0)           a = atan(y/x);
  if (y>=0 && x<0)   a = atan(y/x) + PI;
  if (y<0 && x<0)    a = atan(y/x) - PI;
  if (y>0 && x==0)   a = PI/2.0;
  if (y<0 && x==0)   a =-PI/2.0;
  if (x==0 && y==0)  a = 0;

  return a;
}


task main()
{
  while (true)
  {
    getJoystickSettings(joystick);
    int a = radiansToDegrees(atan2(joystick.joy1_x1,joystick.joy1_y1));
    nxtDisplayString(0, "%i", a);
  }
}