C Code (Controller)
Sorry about the spacing. When it transferred from the editor, the spacing doubled. For better formatting, go here
/* NOTES:
* 0-127 Motor Left (1-63: forward, 64: stop, 65-127: reverse)
* 128-255 Motor Right (128-191: forward, 192: stop, 193-255: reverse)
* Triangle- LEDs
* Start- Remote switch
* Motor left- left joystick
* Motor right- right joystick
* L2- Pan camera left R2- Pan camera right
* Analog- activate controller
*/
#include <GPSXClass.h>
#include <SoftwareSerial.h>
#include <LiquidCrystal.h>
//#include <XBee.h>
#include <Servo.h>
#include <Sabertooth.h>
// LCD SCREEN
#define LCD_COLS 20
#define LCD_ROWS 4
#define BACKLIGHT_ON 'HIGH'
#define BACKLIGHT_OFF 'LOW'
// XBEE
#define DATA_LED 10
#define STATUS_LED 11
#define ERROR_LED 12
#define REMOTE_XBEE_ADDR 0x1874 //TODO
#define RESPONSE_WAIT 5000
#define DEBUG false
// PS2 SEND
#define TRIANGLE 256
#define START 257
#define L2 258
#define R2 259
#define JOYSTICK_LOW 90
#define JOYSTICK_HIGH 160
#define SERIAL_BPS 38400
// GLOBAL VARIABLES
LiquidCrystal lcd(12, 11, 5, 4, 3, 2); //TODO
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
Initialize
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
void setup()
{
initPSX(); //Get PS2 controller ready
initLCD(); //Get LCD ready
initXBee(); //Get XBee Wireless Transmitter ready
}
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
Main
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
void loop()
{
//PS2 Controller Input
readPS2Input();
delay(100);
}
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
Helpers
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
void initPSX()
{
PSX.mode(PSX_PAD1, MODE_ANALOG, MODE_LOCK);
PSX.motorEnable(PSX_PAD1, MOTOR1_DISABLE, MOTOR2_DISABLE);
// Poll current state once.
PSX.updateState(PSX_PAD1);
}
void initLCD()
{
lcd.begin(LCD_COLS, LCD_ROWS);
lcd.clear();
lcd.setCursor(0,0);
//lcd.print("Robot Started");
}
void initXBee()
{
Serial.begin(38400);
}
void readPS2Input()
{
Serial.begin(38400);
PSX.updateState(PSX_PAD1);
if(PRESSED_TRIANGLE(PSX_PAD1))
{
if (DEBUG) Serial.println("Triangle pressed");
else sendData(TRIANGLE);
delay(10);
}
if(PRESSED_START(PSX_PAD1))
{
if (DEBUG) Serial.println("Start pressed");
else sendData(START);
delay(10);
}
if(IS_DOWN_L2(PSX_PAD1))
{
if (DEBUG) Serial.println("L2 down");
else sendData(L2);
delay(10);
}
if(IS_DOWN_R2(PSX_PAD1))
{
if (DEBUG) Serial.println("R2 down");
else sendData(R2);
delay(10);
}
int leftStick = ANALOG_LEFT_Y(PSX_PAD1);
int rightStick = ANALOG_RIGHT_Y(PSX_PAD1);
if(leftStick < JOYSTICK_LOW)
{
if (DEBUG)
{
Serial.print("LEFT TRIGGER FORWARD: ");
Serial.println(ANALOG_LEFT_Y(PSX_PAD1), DEC);
}
else
{
sendData(map(leftStick, 0, JOYSTICK_LOW, 127, 65));
}
}
if(leftStick > JOYSTICK_HIGH)
{
if (DEBUG)
{
Serial.print("LEFT TRIGGER BACKWARDS: ");
Serial.println(ANALOG_LEFT_Y(PSX_PAD1), DEC);
}
else
{
sendData(map(leftStick, JOYSTICK_HIGH, 255, 63, 1));
}
}
if(rightStick < JOYSTICK_LOW)
{
if (DEBUG)
{
Serial.print("RIGHT TRIGGER FORWARDS: ");
Serial.println(ANALOG_RIGHT_Y(PSX_PAD1), DEC);
}
else
{
sendData(map(rightStick, 0, JOYSTICK_LOW, 255, 193));
}
}
if(rightStick > JOYSTICK_HIGH)
{
if (DEBUG)
{
Serial.print("RIGHT TRIGGER BACKWARDS: ");
Serial.println(ANALOG_RIGHT_Y(PSX_PAD1), DEC);
}
else
{
sendData(map(rightStick, JOYSTICK_HIGH, 255, 191, 128));
}
}
}
void sendData(uint16_t sendVal)
{
Serial.print('A');
if (sendVal < 10)
{
Serial.print('0');
Serial.print('0');
}
else if (sendVal < 100)
{
Serial.print('0');
}
Serial.print(sendVal, DEC);
delay(100);
}