#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1, midfollower, sensorLineFollower)
#pragma config(Sensor, in4, leftfollower, sensorLineFollower)
#pragma config(Sensor, in8, rightfollower, sensorLineFollower)
#pragma config(Sensor, I2C_1, leftLift, sensorQuadEncoderOnI2CPort, , AutoAssign)
#pragma config(Sensor, I2C_2, rightLift, sensorQuadEncoderOnI2CPort, , AutoAssign)
#pragma config(Motor, port1, bumperStrafe, tmotorVex269, openLoop, reversed)
#pragma config(Motor, port2, leftDrive, tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor, port3, leftBottomLift, tmotorVex393HighSpeed, openLoop, encoder, encoderPort, I2C_1, 1000)
#pragma config(Motor, port4, leftTopLift, tmotorVex393HighSpeed, openLoop)
#pragma config(Motor, port5, leftFeed, tmotorVex393HighSpeed, openLoop)
#pragma config(Motor, port6, rightFeed, tmotorVex393HighSpeed, openLoop, reversed)
#pragma config(Motor, port7, rightTopLift, tmotorVex393HighSpeed, openLoop, reversed)
#pragma config(Motor, port8, rightBottomLift, tmotorVex393HighSpeed, openLoop, reversed, encoder, encoderPort, I2C_2, 1000)
#pragma config(Motor, port9, rightDrive, tmotorServoContinuousRotation, openLoop)
#pragma config(Motor, port10, bootStrafe, tmotorVex269, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
int midthreshold = 1800;
int leftthreshold= 1800;
int rightthreshold= 1800;
int startleft;
int startright;
void linefollower();
bool verticaldetector();
bool horizontaldetector();
task main()
{
//move to the left until lines are detected
while(verticaldetector()==false)
{
motor[bootStrafe]=60;
motor[bumperStrafe]=-60;
motor[leftDrive]=60;
motor[rightDrive]=60;
}
motor[bootStrafe]=0;
motor[bumperStrafe]=0;
wait1Msec(200);
motor[bootStrafe]=-50;
motor[bumperStrafe]=50;
wait1Msec(200);
motor[bootStrafe]=0;
motor[bumperStrafe]=0;
wait1Msec(200);
startleft = SensorValue[leftLift];
startright = SensorValue[rightLift];
//forward collection and archturn
while(SensorValue[leftLift]-startleft<880&&SensorValue[rightLift]-startright<880)
{
linefollower();
}
motor[leftDrive]=0;
motor[rightDrive]=0;
wait1Msec(200);
//collection here
//arch turn right
motor[leftDrive]=30;
motor[rightDrive]=-30;
motor[bumperStrafe]=60;
motor[bootStrafe]=-60;
wait1Msec(800);
while(verticaldetector()==false)
{
motor[leftDrive]=30;
motor[rightDrive]=-30;
motor[bumperStrafe]=80;
motor[bootStrafe]=-80;
}
motor[leftDrive]=0;
motor[rightDrive]=0;
motor[leftDrive]=-30;
motor[rightDrive]=30;
motor[bumperStrafe]=-60;
motor[bootStrafe]=60;
wait1Msec(100);
motor[leftDrive]=0;
motor[rightDrive]=0;
motor[bumperStrafe]=0;
motor[bootStrafe]=0;
wait1Msec(200);
}
void linefollower()
{
if(SensorValue[midfollower] < midthreshold)
{
motor[leftDrive]=100;
motor[rightDrive]=100;
}
if(SensorValue[leftfollower]<leftthreshold)
{
motor[leftDrive]=30;
motor[rightDrive] = 100;
}
if(SensorValue[rightfollower]<rightthreshold)
{
motor[leftDrive]= 100;
motor[rightDrive] =30;
}
if(SensorValue[midfollower]<midthreshold&&SensorValue[leftfollower]<leftthreshold&&SensorValue[rightfollower]<rightthreshold)
{
motor[leftDrive]= 30;
motor[rightDrive] =-30;
}
}
bool verticaldetector()
{
if(SensorValue[midfollower]<midthreshold&&SensorValue[leftfollower]>leftthreshold&&SensorValue[rightfollower]>rightthreshold)
{
return true;
}
else
return false;
}
bool horizontaldetector()
{
if(SensorValue[midfollower]<midthreshold&&SensorValue[leftfollower]<leftthreshold&&SensorValue[rightfollower]<rightthreshold)
{
return true;
}
else
return false;
}
Here is the Pesudo-Code/Template for what we want Sword to do. Implementation of the code is mainly Elmer's code, so I won't bother showing the C++ version of this:
/*******************************************
PSUEDOCODE
*******************************************/
/*
psuedocode:
From starting position:
deployLift
{
raise lift, 0.5 sec
drive forward,
set lift, lowered
}
findFirstLine
{
Get on the white line
}
followLineToSacks
{
lineFollower - drive forwawrd uses findLine (detect)
sackDetector - detects sac
}
collectionSequence
{
collectFrontSacks
{
set collection, to collect 5 sec
reverse, strafe left, and forward until wall or collect all sacs
strafe right
}
Lift/Dump
{
set lift to raise
set collection to dump sacs for 5 secs
set lift to lowered
}
}
findSecondLine
{
turn 90 clockwise
back-up untill hit wall - Decelleration and pressure sensor
straft right until detect line
}
followLineToSack // again using lineFollower and sackDetector
{
go forward until detect sac
}
collectSequence
{
set collection,to collect 5 sec
}
//Debatable method to dump #1
turn90Arch
{
reverse, strafe, findLine, dump
}
//Debatable method to dump #2
{
back-up until hit the wall - Decelleration and Pressure Sensor
go forward until detect a horizontal line
turn 90 counter-clockwise until detect line
go forward for an x amount of times
Lift/Dump
{
set lift to raise, set colletion to dump sacs for 5 secs
set lift to lowered
}
}
strafe right
straft right for x amount, set collection to collect 5 secs
Lift/Dump
{
set lift to raise, set colletion to dump sacs for 5 secs
set lift to lowered
}
collectionSequence
{
strafe right, drive forward until detects sac
set collectioin to collect 5 sec
forward and reverse
}
strafe left, until our trough
Lift/Dump
{
set lift to raise
set collection to dump sacs for 5 secs
set lift to lowered
turn 90 counter clockwise
}*/
No comments:
Post a Comment