task sweep()
{
while(true)
{
if(joy1Btn(2)&&sweeping==false)
{
sweeping=true;
wait10Msec(50);
}
if(joy1Btn(2)&&sweeping==true)
{
sweeping=false;
wait10Msec(50);
}
while(sweeping==true)
{
motor[sweeper]=100;
}
while(sweeping==false)
{
motor[sweeper]=-100;
}
abortTimeslice();
}
}
task positioning()
{
while(true)
{
if(joy1Btn(6))
{
motor[swinger]=40;
direction=1;
wait10Msec(1);
}
else if(joy1Btn(8))
{
motor[swinger]=-40;
direction=2;
wait10Msec(1);
}
else if(direction==1)
{
motor[swinger]=-10;
wait10Msec(1);
}
else if(direction==2)
{
motor[swinger]=10;
wait10Msec(1);
}
else
{
motor[swinger]=0;
wait10Msec(1);
}
abortTimeslice();
}
}
/*task release()
{
while(true)
{
if(joy1Btn(4)&&open==true)
{
servo[door]=255;
wait10Msec(50);
open=false;
}
if(joy1Btn(4)&&open==false)
{
servo[door]=0;
wait10Msec(50);
open=true;
}
if(joy2Btn(4)&&open==true)
{
servo[door]=255;
wait10Msec(50);
open=false;
}
if(joy2Btn(4)&&open==false)
{
servo[door]=0;
wait10Msec(50);
open=true;
}
abortTimeslice();
}
}*/
task raising()
{
ClearTimer(T1);
while(true)
{
if(joy1Btn(5))
{
motor[pulley]=50;
}
else if(joy1Btn(7))
{
motor[pulley]=-10;
}
else if(joy2Btn(6))
{
motor[pulley]=100;
}
else if(joy2Btn(8))
{
motor[pulley]=-10;
}
else
{
motor[pulley]=0;
}
abortTimeslice();
}
}
task release()
{
while(true)
{
if(joy1Btn(4)&&open==true)
{
servo[door]=255;
wait10Msec(50);
open=false;
}
if(joy1Btn(4)&&open==false)
{
servo[door]=0;
wait10Msec(50);
open=true;
}
if(joy2Btn(4)&&open==true)
{
servo[door]=255;
wait10Msec(50);
open=false;
}
if(joy2Btn(4)&&open==false)
{
servo[door]=0;
wait10Msec(50);
open=true;
}
abortTimeslice();
}
}
task clamping()
{
while(true)
{
/*if(joy1Btn(2)&&clamped==false)
{
servo[clamp1]=255;
wait10Msec(50);
servo[clamp2]=0;
wait1Msec(1);
clamped=true;
}
if(joy1Btn(2)&&clamped==true)
{
servo[clamp1]=0;
wait10Msec(50);
servo[clamp2]=255;
wait1Msec(1);
clamped=false;
}*/
if(joy1Btn(2)&&clamped==false)
{
nMotorEncoder(clamp)=90;
wait10Msec(50);
clamped=true;
}
if(joy1Btn(2)&&clamped==true)
{
nMotorEncoder(clamp)=0;
wait10Msec(50);
clamped=true;
}
abortTimeslice();
}
}
void edgeFollow(bool& running){
sweeps = 0;
if(direction == LEFT) {
motor[left] = -SPEED;
motor[right] = SPEED;
} else {
motor[left] = SPEED;
motor[right] = -SPEED;
}
bool wasDark = isDark();
do{
//Find border point
if(wasDark != isDark()) {
wasDark = isDark();
wait10Msec(3);
if(direction == LEFT) {
direction = RIGHT;
motor[left] = SPEED;
motor[right] = 0;
} else if( direction == RIGHT) {
direction = LEFT;
motor[right] = SPEED;
motor[left] = 0;
}
sweeps++;
abortTimeslice();
}
}while(running);
}
//---------------------------------------------------------
task control()
{
while(true)
{
motor[leftFrontMotor]=LeftFrontDrive;
motor[rightFrontMotor]=RightFrontDrive;
motor[rightBackMotor]=RightBackDrive;
motor[leftBackMotor]=LeftBackDrive;
int DriveFrontLeft=-1*(C1LY - C1LX - C1RX);
int DriveFrontRight=-1*(-C1LY - C1LX - C1RX);
int DriveBackRight=(-C1LY + C1LX - C1RX);
int DriveBackLeft=(C1LY + C1LX - C1RX);
float cubicPowerFrontLeft=cubicMap(DriveFrontLeft);
float cubicPowerFrontRight=cubicMap(DriveFrontRight);
float cubicPowerBackRight=cubicMap(DriveBackRight);
float cubicPowerBackLeft=cubicMap(DriveBackLeft);
RightFrontDrive = cubicPowerFrontRight;
RightBackDrive = cubicPowerBackRight;
LeftFrontDrive = cubicPowerFrontLeft;
LeftBackDrive = cubicPowerBackLeft;
abortTimeslice();
}
}
/**
* Turns around until black is found
*/
void turnToLine(void) {
spinInDirection();
while(!isDark()) {
abortTimeslice();
}
int startDir = currentDirection();
while(isDark()) {
abortTimeslice();
}
motor[left] = 0;
motor[right] = 0;
int centre = startDir + (angleDifference(currentDirection(), startDir)/2);
nxtDisplayCenteredTextLine(6, "cen: %i", centre);
turnToAngle(centre, 0);
}
static void
RCFS_Write( flash_file *f )
{
unsigned short *p;
unsigned short *q;
int i;
volatile FLASH_Status FLASHStatus = FLASH_COMPLETE;
// check for valid address
if( f->addr < baseaddr )
return;
if( f->data == NULL )
return;
// Init pointers (some wierd bug in ROBOTC here)
long tmp = f->addr;
p = (unsigned short *)tmp;
q = (unsigned short *)&(f->name[0]);
// Write header
for(i=0;i<(FLASH_FILE_HEADER_SIZE/2);i++)
{
// Write 16 bit data
FLASHStatus = FLASH_ProgramHalfWord( (uint32_t)p++, *q++ );
}
// point at the file data, we will write as words
q = (unsigned short *)f->data;
// Write Data, datalength is now in bytes so divide datalength by 2
for(i=0;i<(f->datalength/2);i++)
{
// Write 16 bit data
FLASHStatus = FLASH_ProgramHalfWord( (uint32_t)p++, *q++ );
// every 128 bytes abort time slice
if( (i % 128) == 0 )
abortTimeslice();
}
// Was it an odd number of bytes ?
if( (f->datalength & 1) == 1 )
{
// pad with 0xFF and write the last byte
unsigned short b = (*(unsigned char *)q) | 0xFF00;
FLASHStatus = FLASH_ProgramHalfWord( (uint32_t)p++, b);
}
}
task sweeping()
{
while(true)
{
if(joy1Btn(2))
{
servo[door]=10;
}
else if(joy1Btn(1))
{
servo[door]=0;
}
abortTimeslice();
}
}
task release()
{
while(true)
{
if(joy1Btn(4))
{
servo[sweep]=10;
}
else if(joy1Btn(3))
{
servo[sweep]=0;
}
abortTimeslice();
}
}
int FollowLineTilLeaf(void) {
StartTask(FollowEdge);
int forwards = gatherForwardsData();
for(;;) {
if(detectNode(forwards)) {
if(checkIsLeaf()) {
followingEdge = false;
StopTask(FollowEdge);
return FOUND_LEAF;
} else {
forwards = gatherForwardsData();
}
}
abortTimeslice();
}
}
task drive()
{
while(true)
{
float x1 = joystick.joy1_x1;
float y1 = joystick.joy1_y1;
float x2 = joystick.joy1_x2;
float y2 = joystick.joy1_y2;
if(abs(x1)<16)x1=0;
if(abs(y1)<16)y1=0;
if(abs(x2)<16)x2=0;
if(abs(y2)<16)y2=0;
x1*=.787;
x2*=.787;
y1*=.787;
y2*=.787;
int LF = 0;
int RF = 0;
int LB = 0;
int RB = 0;
LF += x1;
RF -= x1;
LB -= x1;
RB += x1;
LF += y1;
RF += y1;
LB += y1;
RB += y1;
LF -= x2;
RF += x2;
LB -= x2;
RB += x2;
motor[driveBL] = LB;
motor[driveFL] = LF;
motor[driveBR] = RB;
motor[driveFR] = RF;
abortTimeslice();
}
}
task sweeping()
{
while(true)
{
if(joy1Btn(2)&&moving==false)
{
motor[motorC]=-100;
wait1Msec(500);
moving=true;
}
else if(joy1Btn(2)&&moving==true)
{
motor[motorC]=0;
wait1Msec(500);
moving=false;
}
abortTimeslice();
}
}
task main()
{
initializeRobot();
waitForStart();
StartTask(Lift);
waitLiftReady();
// We need the hopper to score in rolling goals
StartTask(Hopper);
DriveSpinnerMotor(SPINNER_IN);
// Make us drive safe
setWaitLiftCmd(DRIVE);
servoHookRelease();
// Drive toward the goals and hope we hit one
driveToEncoder(-AUTO_DRIVE_SPEED, 7750);
servoHookCapture();
// Score and wait for the lift to settle
setLiftCmd(MED);
waitLiftAboveRobot();
hopperAutoDump();
while(isLiftAboveRobot()) {
abortTimeslice();
}
//Drive to the goal and turn around outside of it to put the goal in
driveToGyro(35, !TURN_LEFT);
wait1Msec(0.5 * 1000);
driveToEncoder(AUTO_DRIVE_SPEED, 7200);
driveToGyro(175, TURN_RIGHT);
// Release goal
servoHookRelease();
//Drive back towards goal
driveToEncoder(AUTO_DRIVE_SPEED, 7200);
//Set back to collect
setLiftCmd(COLLECT);
}
task raising()
{
while(true)
{
if(joy1Btn(5))
{
motor[pulley]=50;
}
else if(joy1Btn(7))
{
motor[pulley]=-10;
}
else
{
motor[pulley]=5;
}
abortTimeslice();
}
}
task bucket()
{
while(true)
{
if(joy1Btn(6))
{
motor[arm]=40;
wait10Msec(1);
}
else if(joy1Btn(8))
{
motor[arm]=-40;
wait10Msec(1);
}
else
{
motor[arm]=0;
wait10Msec(1);
}
abortTimeslice();
}
}
task raising()
{
while(true)
{
int red=0;
int blue=0;
int green=0;
getRGB(failSafe, red, green, blue);
nxtDisplayTextLine(3, "Red: %d", red);
nxtDisplayTextLine(4, "Green: %d", green);
nxtDisplayTextLine(5, "Blue: %d", blue);
if(joy1Btn(5))
{
motor[pulley]=75;
}
else if(joy1Btn(7))
{
//if(green<40||green>200)
motor[pulley]=-10;
}
else if(joy2Btn(6))
{
motor[pulley]=75;
}
else if(joy2Btn(8))
{
motor[pulley]=-10;
}
else
{
motor[pulley]=10;
}
abortTimeslice();
}
}
task gyro() {
GYRO_READY = false;
GYRO_ANGLE = 0.0;
// Calibrate -- assume we are stationary
float cal = 0.0;
HTGYROsetCal(gyroSensor, 0);
for (int i = 0; i < GYRO_CAL_SAMPLES; i++) {
cal += HTGYROreadRot(gyroSensor);
wait1Msec(5);
}
HTGYROsetCal(gyroSensor, cal / (float)GYRO_CAL_SAMPLES);
// Loop indefinately
time1[T4] = 0;
while (true) {
// Wait in small slices, giving up the CPU
while (time1[T4] < GYRO_PERIOD) {
abortTimeslice();
}
// Immediately reset the timer in case we get de-scheduled
time1[T4] = 0;
// Read and integrate
float speed = readGyroSpeed();
if (abs(speed) < GYRO_FLOAT_SPEED) {
speed = 0.0;
}
GYRO_ANGLE += speed * ((float)GYRO_PERIOD / 1000.0);
GYRO_READY = true;
// Surrender time to other tasks
EndTimeSlice();
}
}
void startGyro() {
StartTask(gyro);
while (!GYRO_READY) {
abortTimeslice();
}
}
