// Thomas Braunl, UWA Aug. 2022
// Note: Custom manipulators have 4 IDs per joint
//       matching the 4 DH parameters
//       Gripper is last ID (here: 3*4=12, so gripper = 13)
#include "eyebot.h"

int main ()
{ int  id, vehicleID=4;

  RADIOInit();
  id = RADIOGetID();
  LCDPrintf("Custom load robot ID %d\n", id);

  SERVOSet( 5, 128);  //extend half 
  LCDPrintf("extend\n"); OSWait(1000);

  while(1)
  { SERVOSet( 1,  20);  //lower
    LCDPrintf("lower\n"); OSWait(1000);
    SERVOSet(13, 255);  //gripper
    LCDPrintf("grip\n"); OSWait(1000);
    SERVOSet( 1, 150);  //raise
    LCDPrintf("raise\n"); OSWait(1000);
    // now holds can

    SERVOSet( 5, 255);  //extend full
    LCDPrintf("extend\n"); OSWait(1000);
    SERVOSet( 1,  70);  //lower
    LCDPrintf("lower\n"); OSWait(1000);
    SERVOSet(13,   0);  //gripper
    // now dropped onto vehicle

    LCDPrintf("grip\n"); OSWait(1000);
    SERVOSet( 1, 100);  //raise
    LCDPrintf("raise\n"); OSWait(1000);
    SERVOSet( 5,   0);  //extend back
    LCDPrintf("messaging vehicle ID %d to start\n", vehicleID);
    RADIOSend(vehicleID, "Start");
    // any message will start vehicle
 
    LCDPrintf("extend\n"); OSWait(1000);
  }
  return 0;
}
