Intake Test RobotC code

This code was used to run the motors for a prototype intake mechanism with a left and right motor.  A single switch was used to start, stop and reverse the motors.  The Vex .5 ucontroller was used.

#pragma config(ProgramType, StandaloneWithWiFi)
#pragma config(Sensor, in1,    button,              sensorTouch)
#pragma config(Motor,  port1,           lt_intake,     tmotorNormal, openLoop)
#pragma config(Motor,  port2,           rt_intake,     tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by ‘ROBOTC’ configuration wizard               !!*//
// Code written by Vamfun for Gateway: Oct 31, 2011
// This program provides a single button control of a prototype intake.
//Push and hold the button for 2 seconds and release turns on the motors.
//Push and hold the button for 2 seconds and release turns off the motors if running.
//Push and hold the button for short interval (2 milli seconds) and reverses the direction.
bool timer_started = false;
bool on = false;
int sw = 0;
int sw_last = 0;
int cmd = 0;

task main ()
{
while(true)
{
sw = SensorValue[button];

if(!timer_started)
{
time1[T1] = 0;
}
if (!sw_last && sw && !timer_started )

{

timer_started = true;
}
if(on &&!sw && sw_last && timer_started && time1[T1]> 2)
{

cmd = – cmd;
timer_started = false;

}
if(sw && timer_started && time1[T1] > 2000)
{
timer_started = false;
if( !on )
{
on = true;
cmd = 127;
}
else
{
on = false ;
cmd = 0;
}

}

 

motor[lt_intake] = cmd;
motor[rt_intake] = cmd;

sw_last = sw;
}

}