/*************** Joystick Read Function **********/
void read_joystick()
{
while(1) // Stay reading the joystick until it has fully been read 1 time
{
incoming_byte = Serial1.read(); // Read the byte and set it equal to incoming_byte
if (incoming_byte == delimiter) // If incoming byte is the delimiter
{
if (index == 3) // If we have also not read junk code
{
md.setM1Speed((joystick_x + joystick_y)/(2 - turbo_mode)); // Linear algebra equation to drive motor
md.setM2Speed((-joystick_x + joystick_y)/(2 - turbo_mode));
Serial1.flush(); // Clear the buffer from the incoming serial data
index =0;
break;
} //end if(index ==3)
index =0;
skip_case =1;
} //end if(incoming_byte==delimiter)
switch(skip_case)
{
case 0:
switch(index)
{
case 0: //If index == 0
joystick_x= (((int(incoming_byte)-63)*470)/75); // mapped joystick x from -400 to 400 (for Motor Voltages)
index++;
break;
case 1: //If index == 1
joystick_y = (((int(incoming_byte)-63)*470)/75); // mapped joystick y from -400 to 400 (for Motor Voltages)
index++;
break;
case 2: //If index == 2
switch(int(incoming_byte))
{
case 32: //No buttons have been pressed
turbo_mode = false;
break;
case 1:
//Serial.println("Left Button Press");
break;
case 2:
//Serial.println("Top Button Press");
arm_function();
break;
case 3:
//Serial.println("Right Button Press");
grab_function();
break;
case 4:
//Serial.println("Bottom Button Press");
turbo_mode = true;
break;
case 5:
//Serial.println("Select Button Press");
break;
}
index++;
break;
}
case 1:
skip_case = 0;
} //end skip_case
} //end while(1)
} //end joystick function