#include "NXTMMX-lib.nxc"
#define SensorPort S3
int Addr = 6;
task LowerFork();
task LiftFork();
task main()
{
 SetSensorTouch(IN_1);
 SetSensorTouch(IN_2);
 SetSensorLowspeed(IN_3);
 SetSensorTouch(IN_4);
 MMX_Init( SensorPort, Addr, 0 );
 NumOut(0,LCD_LINE1,Sensor(IN_1));
 NumOut(0,LCD_LINE2,Sensor(IN_2));
 NumOut(0,LCD_LINE3,SensorUS(IN_3));
 NumOut(0,LCD_LINE4,Sensor(IN_4));
 Precedes(LiftFork);
 PlayTone(TONE_C4, MS_500);

 MMX_Run_Unlimited(SensorPort, Addr, MMX_Motor_Both, MMX_Direction_Forward, MMX_Speed_Full);
 RotateMotorEx(OUT_AC, 100, 1114, 0, true, true);
 RotateMotorEx(OUT_AC, 100, 470, -100, true, true);
 RotateMotorEx(OUT_AC, 100, -1910, 0, true, true);
 RotateMotorEx(OUT_AC, 100, 470, 100, true, true);
 while (Sensor(IN_4)==0)
 {
  OnFwdSync(OUT_AC, -100, 0);
 }
 Wait(500);
 Off(OUT_AC);
 MMX_Run_Unlimited(SensorPort, Addr, MMX_Motor_Both, MMX_Direction_Reverse, MMX_Speed_Full);
 Wait(4000);
 Precedes(LowerFork);
 PlayTone(TONE_C5, MS_500);
}
task LowerFork()
{
  while (Sensor(IN_1)==0)
  {
  OnFwd(OUT_B, 100);
  }
  Off(OUT_B);
}
task LiftFork()
{
 RotateMotor(OUT_B, 100, -7200);
}