//This Script along with the AutoPosition Action it's attached to will move
//both the DC Elbow motors to center. The AutoPosition will move Wrist 
//Servos to cneter. This sends commands to the Kangaro to move both Elbow motors.
//#####################################################
//Elbow Kangaroo Values:
//Uppercase "P" returned means Movment has stopped
//LOwercase "p" means movment is underway
//
//Right Elbow Motor:
//p3000-Full up, p2470-Center, p1885-Full Down
//s2000-Full Speed, s0-Dead Stop
//
//Left Elbow Motor:
//p1885-Full up, p2490-Center, p3000-Full Down
//s2000-Full Speed, s0-Dead Stop
//#######################################################
//---------------------------------------

//$Both_Arms_Centered = 1 #Reset varable so script will run. 1=not centered, 0=centered

//Move Left Elbow motor to Center
UART.hardwareUartWriteString(0,"2,P2490 s1600\r",2); //Connected to UART 0 on EZB 2

//Move Right Elbow motor to Center
UART.hardwareUartWriteString(0,"1,P2470 s1600\r",2); //Connected to UART 0 on EZB 2

sleep(500)//Pause to let motors start moving. Script needs motors moving to working correctly.

//***********************
//Left Elbow Stop Movment and Position Check
//***********************
// Pauses Script and loops until a capital P is returned (Movment Stopped) then
//returns the Left Elbow positionon.  UART #0 of EZB #2
// --------------------------------
// loop forever because we BREAK out of this loop when P is
// capital
while (true) {
var lft_elbow_position = get_lft_elbow_MovementValue();
  
  // If the position is zero or greater, we know there's a position
  // returned
  if (lft_elbow_position > 0) {
  
print("Left Elbow stopped moving and the position is: " + lft_elbow_position);
break;
  }
 //sleep(250);
 
 }
// This function will return the value of the position if
// moving has completed.
// OR it will return -1 if the moving is still happening
function get_lft_elbow_MovementValue() {

// Get Left Channel ARS Kangaroo position
UART.hardwareUartWriteString(0, "2, Getp\r", 2);

sleep(100)

  // get bytes avaiable on UART #0, EZB #2
  var avail = UART.hardwareUartAvailable(0, 2);

  // If we don't have at least 9 characters, return -1
  if (avail < 9)
 return -1;

  // read 9 bytes from UART #0 on EZB #2
  var resp = UART.hardwareUartReadString(0, 9, 2);

  // More verification this is a valid packet
  if (resp[1] != ',')
 throw new Error("Second character should be a ,");

  // Check if we have a capital P, if so, extract
  // the position and return the new value
  if (resp[2] == 'P') {

  // get the characters between positions 3 & 7 of the string
  var tmpPart = resp.substring(3, 7);

  // convert the string to an int
  var tmpInt = parseInt(tmpPart);
  
  // return the position
  return tmpInt;
  }

  // still moving 
  return -1;
}

sleep(100)

//************************
//Right Elbow Stop Movment and Position Check
//************************
//Pauses Script and loops until a capital P is returned (Movment Stopped) then
//returns the Right Elbow positionon. on UART #0 of EZB #2
// --------------------------------
// loop forever because we BREAK out of this loop when P is capital
while (true) {

  var Rt_elbow_position = get_Rt_elbow_MovementValue();
  
  // If the position is zero or greater, we know there's a position
  // returned
  if (Rt_elbow_position > 0) {
print("Right Elbow stopped moving and the position is: " + Rt_elbow_position);
break;
  }
 
 }
 
// This function will return the value of the position if
// moving has completed.
// OR it will return -1 if the moving is still happening
function get_Rt_elbow_MovementValue() {

// Get Right Channel ARS Kangaroo position
UART.hardwareUartWriteString(0, "1, Getp\r", 2);

sleep(100)

  // get bytes avaiable on UART #0, EZB #2
  var avail = UART.hardwareUartAvailable(0, 2);

  // If we don't have at least 9 characters, return -1
  if (avail < 9)
 return -1;

  // read 9 bytes from UART #0 on EZB #2
  var resp = UART.hardwareUartReadString(0, 9, 2);

  // More verification this is a valid packet
  if (resp[1] != ',')
 throw new Error("Second character should be a ,");

  // Check if we have a capital P, if so, extract
  // the position and return the new value
  if (resp[2] == 'P') {

  // get the characters between positions 3 & 7 of the string
  var tmpPart = resp.substring(3, 7);

  // convert the string to an int
  var tmpInt = parseInt(tmpPart);
  
  // return the position
  return tmpInt;
  }

  // still moving 
  return -1;
}

//*********************
//Check each elbow's OPto Switches are closed. Auto Center elbows if needed to close opto
//*********************

//Check Left Elbow Opto Switch closed and AutoCenter if needed
//If closed we know the arm is centered and in position.
//**********************
//This Var Sets the start position away from center to start countdown back to center
var lft_elbow_AutoCenterStart = 2700 
sleep(100)

//Looks at target ADC reading of Opto Switch for Elbow center. 
//Above 185 to 255 is considdered center. If not embow will try to auto center
if (ADC.get(1, 2) < 185) { //Start Auto Center if ADC is below 185. Not centered.
sleep(100)
print("Left Arm Not Centered! Try to calibrate.")
ControlCommand("Soundboard 2", "Track_20"); //Say "Please Stand By"
sleep(2500)

 // Command moves the elbow to different start position away from center to start countdown back to center 
UART.hardwareUartWriteString(0,"2,P" + (lft_elbow_AutoCenterStart) + "s1600\r" ,2);
sleep( 1000 );

do{
var Left_Elbow_ADC = ADC.get(1, 2)//Start Auto Center if ADC is below 185
UART.hardwareUartWriteString(0,"2,P" + (lft_elbow_AutoCenterStart-=10) + "s1000\r" ,2); //Connected to UART 0 on EZB 2
sleep(1000);
print("ADC Read: " + Left_Elbow_ADC)
sleep(100);
}while(Left_Elbow_ADC < 185); //Once ADC gets above 185, script will continue
sleep(100);
print("ADC Final Read: " + Left_Elbow_ADC)//Get ADC Value on ADC Port 1 from EZB 2 and Print
//ControlCommand("Soundboard v4", "Track_86"); //Say "conclusion"
sleep(500)
var lft_elbow_position = get_lft_elbow_MovementValue();
print("Left Arm Centered! Safe to Retract!"); //Left Arm Centered! Safe to Retract. Fully cnetered should read about 255
print("Left Arm ADC Reading: " + Left_Elbow_ADC)
print("Left Elbow Final Position: " + lft_elbow_position);

}

//***********************
//Right Elbow Opto Switch center check and AutoCenter if needed
//***********************
//Check the Right Elbow opto sensor ADC value to make sure it's closed. 
//If Closed we know the arm is centered and in position. 

//This Var Sets the start position away from center to start countdown back to center
var Right_elbow_AutoCenterStart = 2250 
sleep(100)

//Looks at target ADC reading of Opto Switch for Elbow center. 
//Above 185 to 255 is considdered center. If not embow will try to auto center
if (ADC.get(0, 2) < 185) { //Start Auto Center if ADC is below 185
sleep(100)
print("Right Arm Not Centered! Try to calibrate.")
ControlCommand("Soundboard 2", "Track_20"); //Say "Please Stand By"
sleep(2500)

// Command moves the elbow to different start position away from center to start countdown back to center 
UART.hardwareUartWriteString(0,"1,P" + (Right_elbow_AutoCenterStart) + "s1600\r" ,2);
sleep( 1000 );

do{
var Right_Elbow_ADC = ADC.get(0, 2)
UART.hardwareUartWriteString(0,"1,P" + (Right_elbow_AutoCenterStart+=10) + "s1000\r" ,2); //Connected to UART 0 on EZB 2
sleep(1000);
print("ADC Read: " + Right_Elbow_ADC)
sleep(100);
}while(Right_Elbow_ADC < 185); //Pause Script but once ADC gets above 185, script will continue
sleep(100);
print("ADC Final Read: " + Right_Elbow_ADC)//Get ADC Value on ADC Port 1 from EZB 2 and Print
//ControlCommand("Soundboard v4", "Track_86"); //Say "conclusion"
sleep(750)
var Right_elbow_position = get_Rt_elbow_MovementValue();
print("Right Arm Centered! Safe to Retract!"); //Left Arm Centered! Safe to Retract. Fully cnetered should read about 255
print("Right Arm ADC Reading: " + Right_Elbow_ADC)
print("Right Elbow Final Position : " + Right_elbow_position);

}

print("Both Arms Centered. Ok to retract")