// TODO: use correct sensor port (change to irBack)
task main() {
calibrateGyro();
{ // Drive to the IR basket, dump the block, drive back & square up against the wall
int timeForward;
ClearTimer(T1);
driveBackward(DRIVE_SPEED);
while (true) {
if (time1[T1] > SEARCH_TIME)
break;
else if (SensorValue[irFront] == 2)
break;
}
timeForward = time1[T1];
// Keep moving to adjust for the first 2 / last 2 baskets
if (timeForward < MIDDLE_TIME) {
// Back up a little
driveStop();
wait1Msec(500);
driveForward(FIRST_HALF_DELAY, DRIVE_SPEED);
timeForward -= FIRST_HALF_DELAY;
} else {
// Back up a little
driveStop();
wait1Msec(500);
driveForward(SECOND_HALF_BACKUP_TIME, DRIVE_SPEED);
timeForward -= SECOND_HALF_BACKUP_TIME;
}
driveStop();
flipper_flip();
driveForward(timeForward + 1500, DRIVE_SPEED);
motor[rightDrive] = DRIVE_SPEED;
wait1Msec(500);
driveStop();
}
{ // Drive next to the ramp & turn on to it
motor[leftDrive] = -DRIVE_SPEED;
wait1Msec(1050);
motor[rightDrive] = -DRIVE_SPEED;
wait1Msec(1300);
driveStop();
turnRightEuler(TURN_90_EULER, DRIVE_SPEED);
PlaySound(soundBeepBeep);
driveBackward(1500, DRIVE_SPEED);
}
}
void navigate()
{
extern enum driveState tess_drive_state;
tess_ping = ping_cm(ULTRASONIC_PIN);
debug("Distance = %i\n", tess_ping);
if (tess_ping == 0 && tess_drive_state != IDLE)
{
tess_drive_state = IDLE;
debug("Ultrasonic Error!\n", 0);
setServo(0,0,"Error: in void navigate()\n");
}
else
{
if (tess_ping < OBJECT_DISTANCE && tess_drive_state != TURNING)
{
//TODO stop and turn
stop();
debug("Turning on LED!\n", 0);
high(LED_1);
debug("About to turn!\n", 0);
turn(LEFT); //enum from drivetrain
}
else if(tess_ping >= OBJECT_DISTANCE && tess_drive_state != MOVING)
{
driveForward(-1);
low(LED_1);
}
}
}
task main()
{
while(true)
{
readSensors();
if (touchL != 0 || touchR != 0)
{
avoidObstacle();
}
else if (lightDetected && facingLight)
{
driveForward();
}
else if (lightDetected && !facingLight)
{
turnTowardsLight();
}
else
{
//Default case
scanForLight();
}
}
}
void reverse15Red()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1550;
int pot = analogRead (8);
int encoder1 = 1200; //drive under wall
int encoder2 = 136; // rotate 90 degrees
int encoder3 = 1300; // backwards to the opponets goal
int encoder4 = 900; //turn to bridge
int encoder5 = 200; // knock their wallball
int encoder6 = 200; // turn to other ball
int encoder7 = 400; // knock their center ball
int encoder8 = 400; // positioning
// begin routine
intakeDead(); // unfold
delay(300); // needs to clear the wall..
driveBack (encoder1); // drive backwards to under the bridge
stopIntake();
turnRight (encoder2); // turn ass to opponets goal
driveBack (encoder3); // drive to opponets goal
driveBackDead(); // 90 deg align
delay(400);
stopDrive ();
armUp(wallHeight);// arm up
scrapeLeft (encoder4); // turn to wall ball
driveForward (encoder5); // knock off wall ball
scrapeLeftBack(encoder6); // turn to middle ball
driveForward(encoder7); // knock off middle ball
driveBack (encoder8); // positioning
stopAll ();
}
void kakitRed ()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1550;
int dead1 = 1000;
int dead2 = 2000;
int dead3 = 3000;
int pot = analogRead (8);
int encoder1 = 1000;
int encoder2 = 150;
int encoder3 = 2000;
int encoder4 = 75;
int encoder5 = 75;
int encoder6 = 75;
int encoder7 = 75;
int encoder8 = 75;
int encoder9 = 75;
// begin routine
driveForwardDead (); //ram big balls
intakeDead ();
delay (3000);
stopIntake();
driveBackDead (4000); // wall align to 90 deg
driveForward (encoder1);
turnRight (encoder2); // turn towards buckys
intakeDead ();
//line follow forward (encoder 4)
driveBackSlowDead (); // allign the bump
driveBackDead(); // over the bump
driveForwardSlowDead(); // alighn to bump
driveBackSlow(encoder4);
turnRight (encoder5);
driveBackSlow (encoder6) ; // go under the bridge
armUpDead (); // break the bridge
delay(500);
armDown (armMin);
driveBack (encoder7);
turnRight (encoder8);
armUp (goalHeight);
//line follow (encoder9);
outtake(8000); // score all three balls in the goal
stopAll ();
}
// ----------------------------------------------------------------------------
// drive using light ambient sensors for directional command
// ------------------------------------------------------------[ driveByLight ]
void WRTbotDrive::driveByLight( wrt_motor_t &motor )
{
// -------------------------------------------------------------------
if( ( 0 == (motor.left.control & wrt_motorControl_power_bit) ) ||
( 0 == (motor.right.control & wrt_motorControl_power_bit) ) )
{
WRTbotDrive::stop();
return;
}
// --- read ambient light sensor
// -------------------------------------------------------------------
sensor.light.data = analogRead( wrt_pin_ambientLight_sensor );
Serial.println(" ");
Serial.print( sensor.light.data ); Serial.print( " , " );
// --- convert to ambient light units
// -------------------------------------------------------------------
sensor.light.data *= sensor.light.factor;
Serial.print( sensor.light.data ); Serial.println( " , " );
useNominal = true;
// -------------------------------------------------------------------
if( (sensor.light.data > sensor.light.limitMin) && // minimum limit
(sensor.light.data < sensor.light.limitLowerBound) ) // highest lower bound
{
Serial.print( "driveLeft" ); Serial.println( " " );
driveLeft( motor );
}
else if( (sensor.light.data > sensor.light.limitUpperBound) && // lowest upper bound
(sensor.light.data < sensor.light.limitMax) ) // maximum limit
{
Serial.print( "driveRight" ); Serial.println( " " );
driveRight( motor );
}
else
{
Serial.print( "driveForward" ); Serial.println( " now " );
driveForward( motor );
}
} // done driveByLight()
////////////////////////////////////////////////////////////////////////////////////
// Autonomous Routine supports "controlled" running, to allow steps to be replayed,
// Speeds adjusted, and more.
////////////////////////////////////////////////////////////////////////////////////
void our_auto(bool autoRun){
writeDebugStreamLine("****** Starting Autonomous Function **********************");
int stepCounter = 0;
if(!autoRun){
writeDebugStreamLine("--- AutoRun == false ---- ");
}
// Increment this when there's more steps!
while(stepCounter < 20){
writeDebugStreamLine("--Step number: %d", stepCounter);
getStepSelection(stepCounter, autoRun);
// .....................................................................................
// This is the OFFICIAL Autonomous Routine. Experiments go in a "SimpleTestHarness"
// .....................................................................................
switch(stepCounter){
case 0:
driveForward(1005, 100);
break;
case 1:
spinLeft(45, 100);
break;
case 2:
driveForward(1000, 100);
break;
case 3:
spinLeft(90, 100);
break;
default:
writeDebugStreamLine("--Step number %d not defined", stepCounter);
}
stepCounter++;
} // End While loop
writeDebugStreamLine("****** Completed Autonomous Function **********************");
}
void Motor_run()
{
if(MPI_check_available(MOTOR_DRIVER_PID) == true)
{
MPI_get_message(MOTOR_DRIVER_PID, &incomingPID, &cmdSize, incomingMsg);
if(cmdSize == sizeof(int))
{
OS_memcpy(&incomingCmd, incomingMsg, cmdSize);
switch(incomingCmd)
{
case C_DRIVE_FORWARD:
UART_send("drive forward\r\n", CONSOLE_BASE);
driveForward();
break;
case C_DRIVE_REVERSE:
UART_send("drive reverse\r\n", CONSOLE_BASE);
driveReverse();
break;
case C_DRIVE_LEFT:
UART_send("drive left\r\n", CONSOLE_BASE);
driveLeft();
break;
case C_DRIVE_RIGHT:
UART_send("drive right\r\n", CONSOLE_BASE);
driveRight();
break;
case C_DRIVE_HALT:
UART_send("drive halt\r\n", CONSOLE_BASE);
driveHalt();
break;
}
}
}
}
void avoidObstacle()
{
driveBackward();
wait1Msec(1000);
if (touchL != 0)
{
lastHitDir = -1;
rightDeg(45);
}
else if (touchR != 0)
{
lastHitDir = 1;
leftDeg(45);
}
driveForward();
wait1Msec(2000);
}
int main(){
while(1){
switch(state)
{
case 1:
if(digitalRead(FRONT)==1)
{ goUntilLROn();
turnDirection(right);
}
if(digitalRead(RIGHT)==1)
{
driveForward();
}
else
turnRight(10);
default : break;
}
}
}
void classic15Blue()
{
int armMin = 290;
int wallHeight = 1000;
int goalHeight = 1700;
int dead1 = 1200;
int dead2 = 2000;
int dead3 = 3000;
int pot = analogRead(8);
//encoder values
int encoder1 = 900; //drive to goal
int encoder2 = 325; //keep going towards goal
int encoder3 = 0; //drive slow, adjust to 90 degrees
int encoder4 = 325; //back up
int encoder5 = 1350; //back up across the barrier again
int encoder6 = 80; // turn towards center large ball
int encoder7 = 600; // hit 1st ball off the barrier
int encoder8 = 350; // drive back
int encoder9 = 200; // turn towards other large ball
int encoder10 = 400; // hit 2nd ball off the barrier
int encoder11 = 300; // drive back to square
int encoder12 = 290; // turn to barf
int encoder13 = 800; // drive to barf + pickup
int encoder14 = 400;
int encoder15 = 700;
// begin routine
intakeDead();
delay(1000);
stopIntake();
// driveforward (some curve)////////////////////////////////////////////////////
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < encoder1)
{
motorSet(MOTOR_DRIVE_RIGHT_BACK, 110); // slight curve cuz friction
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 110);
motorSet(MOTOR_ARM_LEFT_BACK, 127);
motorSet(MOTOR_ARM_LEFT_FRONT, 127);
imeGet(0, &counts); // keep getting the value
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
motorSet(MOTOR_ARM_LEFT_BACK, 0);
motorSet(MOTOR_ARM_LEFT_FRONT, 0);
delay(200);
///////////////////////////////////////////////////////////////////////////////
//driveForward(encoder1); // drive to goal
armUp(goalHeight); //raise arm after cross barrier
driveForwardSlow(encoder2); //keep going towards goal
driveForwardSlowDead(); //drive slow, adjust to 90 degrees
delay(1000);
outtake(1700); // outtake
driveBack(encoder4); // back up
delay(300);
armDown(armMin); // lower arm
driveBack(encoder5); //back up across the barrier again
delay(300);
turnLeft(encoder6); // turn towards center large ball
armUp(wallHeight); // arm up to wall height
driveForward(encoder7); // hit it off the barrier
delay(500);
driveBack(encoder8); // drive back
delay(300);
turnRight(encoder9); // turn towards other large ball
driveForward(encoder10); // hit 2nd ball off the barrier
delay(500);
driveBack(encoder11); // drive back to square
delay(600);
turnLeft(encoder12); // turn to barf
delay(300);
armDown(armMin);
intakeDead(); // pick up barf
driveForward(encoder13); // drive towards barf + intake it
delay(500);
//end of routine
stopAll();
delay(60000); ///////////////////////////////////////////////////////////////////////////////////
}
/****** AUTO FUNCTIONS END *******/
void Autonomous()
{
int counter=0;
int autonomousEngagement = 0;
DriverStationLCD *screen = DriverStationLCD::GetInstance();
compressor.Start(); //starts compressor class
rightArmSolenoid.Set(DoubleSolenoid::kReverse); //brings the arms down
leftArmSolenoid.Set(DoubleSolenoid::kReverse);
/*** ENSURES THE CATAPULT IS LOADED AND LOADS IF UNLOADED ***/
if (leftLimitSwitch.Get() == 1 && rightLimitSwitch.Get() == 1)
{
winchMotor.Set(0.1); // Gears need to be moving slowly to allow the dog gear to engage properly
dogSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to engage the dog gear
Wait(0.2); // Giving the pistons time to engage properly
winchMotor.Set(0); // Now that the dog gear is engaged, the gears do not have to move
ratchetSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to engage the ratchet
Wait(0.2); // Giving the pistons time to engage properly
}
while (leftLimitSwitch.Get() == 1 && rightLimitSwitch.Get() == 1) // If Limit Switch Buttons are not pressed
{
winchMotor.Set(1); //Now starts the winch motor to load the catapult
}
// If the Catapult Left & Limit Switches are (0,0), (0,1), (1,0)
{
winchMotor.Set(0); // Stops the Winch Motor since one or more buttons are pressed
if ((dogSolenoid.Get() == DoubleSolenoid::kReverse) && (ratchetSolenoid.Get() == DoubleSolenoid::kForward)) // If the Dog Gear is disengaged but the ratchet is engaged
{
winchMotor.Set(0.05); // Gears need to be moving slowly to allow the dog gear to engage properly. Might want to test this since the catapult's already loaded.
dogSolenoid.Set(DoubleSolenoid::kForward); // Engages the dog gear so both dog gear and ratchet are engaged before shooting for safety
Wait(0.1); // Giving the pistons time to engage properly
winchMotor.Set(0); // Now that the dog gear is engaged, the gears do not have to move
}
else if ((dogSolenoid.Get() == DoubleSolenoid::kForward) && (ratchetSolenoid.Get() == DoubleSolenoid::kReverse)) // If the dog gear is engaged but the ratchet is disengaged
{
ratchetSolenoid.Set(DoubleSolenoid::kForward); // Engages the ratchet so that both dog gear and ratchet are engaged before shooting for safety
Wait(0.1); // Giving the pistons time to engage properly
}
}
/*** DONE LOADING THE CATAPULT ***/
float pLower = 5; // min height of rectangle for comparison
float pUpper = 15; // max height of rectangle for comparison
int criteriaCount = 1; // number of elements to include/exclude at a time
int rejectMatches = 1; // when set to true, particles that do not meet the criteria are discarded
int connectivity = 1; // declares connectivity value as 1; so corners are not ignored
int filterFunction; // removes small blobs
int borderSetting; // variable to store border settings, limit for rectangle
int borderSize = 1; // border for the camera frame (if you don't put this, DriverStation gets mad at you)
ParticleFilterCriteria2 particleCriteria;
ParticleFilterOptions2 particleFilterOptions;
int numParticles;
particleCriteria.parameter = IMAQ_MT_BOUNDING_RECT_HEIGHT; //The Morphological measurement we use
particleCriteria.lower = pLower; // The lower bound of the criteria range
particleCriteria.upper = pUpper; // The upper bound of the criteria range
particleCriteria.calibrated = FALSE; // We aren't calibrating to real world measurements. We don't need this.
particleCriteria.exclude = TRUE; // Remove all particles that aren't in specific pLower and pUpper range
particleFilterOptions.rejectMatches = rejectMatches; // Set to 1 above, so images that do not meet the criteria are discarded
particleFilterOptions.rejectBorder = 0; // Set to 0 over here so border images are not discarded
particleFilterOptions.connectivity8 = connectivity; // Sets the image image to 8 bit
while ((IsAutonomous()))
{
if (logitech.GetRawButton(4))
{
autonomousEngagement = 1;
}
if (autonomousEngagement == 0) // If real autonomous is not engaged start
{
if (logitech.GetRawButton(1))
{
driveForward();
}
if (logitech.GetRawButton(9))
{
dogSolenoid.Set(DoubleSolenoid::kForward); // Brings the pneumatic piston backward to raise the retrieval arm
winchMotor.Set(0.1);
Wait(0.3);
ratchetSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to lower the retrieval arm
while(leftLimitSwitch.Get()==1 && rightLimitSwitch.Get()==1)
{
winchMotor.Set(1);
}
}
if (logitech.GetRawButton(2))
{
autonomousCatapultRelease();
}
if (logitech.GetRawButton(3))
{
stopDriving();
}
if (logitech.GetRawButton(5))
{
turnLeft();
}
if (logitech.GetRawButton(7))
{
turnLeftMore();
}
if (logitech.GetRawButton(6))
{
turnRight();
}
if (logitech.GetRawButton(8))
{
turnRightMore();
}
}// If real autonomous is not engaged end
HSLImage* imgpointer; // declares an image container as an HSL (hue-saturation-luminence) image
imgpointer = camera.GetImage(); //tells camera to capture image
ringLight.Set(Relay::kForward); //turns ringlight on
BinaryImage* binIMG = NULL; // declares a container to hold a binary image
binIMG = imgpointer -> ThresholdHSL(0, 255, 0, 255, 235, 255); // thresholds HSL image and places in the binary image container
delete imgpointer; // deletes the HSL image to free up memory on the cRIO
Image* modifiedImage = imaqCreateImage(IMAQ_IMAGE_U8, 0); //create a binary 8-bit format shell for the image
filterFunction = imaqParticleFilter4(modifiedImage, binIMG -> GetImaqImage(), &particleCriteria, criteriaCount, &particleFilterOptions, NULL, &numParticles); //The Particle Filter Function we use. (The ones before it are outdated)
borderSetting = imaqSetBorderSize(modifiedImage, borderSize); // Sets a border size so DriverStation is happy
delete binIMG; //Deletes the Binary image
int functionCountParticles; // stores number of particles
int particleAmount; // stores the number of particles for the measure particle function
functionCountParticles = imaqCountParticles(modifiedImage, TRUE, &particleAmount); // Counts the number of particles
int functionOne; // The first measuring particle function (specifically for particle #1)
int functionTwo; // The second measuring particle function (specifically for particle #2)
double particleOneOrientation; // TRULY ARBITRARY name of the first particle it find
double particleTwoOrientation; // TRULY ARBITRARY name of the second particle it finds
functionOne = imaqMeasureParticle(modifiedImage, 0, FALSE, IMAQ_MT_ORIENTATION, &particleOneOrientation); // Measures orientation of particle 1
functionTwo = imaqMeasureParticle(modifiedImage, 1, FALSE, IMAQ_MT_ORIENTATION, &particleTwoOrientation); // Measures orientation of particle 2
screen->PrintfLine(DriverStationLCD::kUser_Line2,"P1: %f", particleOneOrientation); // Prints particle 1's orientation
screen->PrintfLine(DriverStationLCD::kUser_Line3,"P2: %f", particleTwoOrientation); // Prints particle 2's orientation
imaqDispose(modifiedImage); // Deletes the filtered image
/**LEFT POSITION**/
if ((leftPositionSwitch.Get() == 1) && (rightPositionSwitch.Get() == 0)) // Left switch set on, switch set off
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Left Position:F"); // Left position and facing forward
if ((particleOneOrientation > 0 && particleOneOrientation < 10) || (particleTwoOrientation > 0 && particleTwoOrientation < 10))
// The target should be hot. Now it goes to the other goal.
/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Left Position Hot!");
// These DEFINITELY need to be tested. All of them. Forreal.
turnRight();
//driveForward();
Wait(3);
stopDriving();
//autonomousCatapultRelease();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Left Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnRight();
driveForward();
Wait(4);
stopDriving();
//autonomousCatapultRelease();
}
}
/**CENTER POSITION**/
else if ((leftPositionSwitch.Get() == 0) && (rightPositionSwitch.Get() == 0)) // Left switch off and right switch off
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position:R"); // Middle position and facing
if ((particleOneOrientation > 0 && particleOneOrientation < 10) || (particleTwoOrientation > 0 && particleTwoOrientation < 10)) // The target should be hot. Now it goes to the other goal.
/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnLeftMore();
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
}
/** RIGHT POSITION**/
else if ((leftPositionSwitch.Get() == 1) && (rightPositionSwitch.Get() == 1))
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position:R"); // Middle position and facing
if ((particleOneOrientation > 0 && particleOneOrientation < 10) || (particleTwoOrientation > 0 && particleTwoOrientation < 10)) // The target should be hot. Now it goes to the other goal.
/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnLeftMore();
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
}
else if (((leftPositionSwitch.Get()) == 1) && ((rightPositionSwitch.Get()) == 0)) // Left switch off and switch on
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Right Position"); // position and facing forward
if ((particleOneOrientation > 0 && particleOneOrientation < 10) || ((particleTwoOrientation > 0) && (particleTwoOrientation < 10))) // The target should be hot. Now it goes to the other goal.
/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Right Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnLeft();
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4, "Right Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
}
counter++;
screen -> PrintfLine(DriverStationLCD::kUser_Line5,"R: %f L: %f)", rightFront.Get(), leftFront.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Counter %d", counter);
screen->UpdateLCD();
}
compressor.Stop();
}
task main() {
servoPrep();
Joystick_WaitForStart();
disableDiagnosticsDisplay();
while(true) {
encoderPrep();
startTrackers();
driveForward(310.0);
turnRight(85.0);
if(irDetected == true) {
irPos2 = true;
turnRight(40.0);
irDetected = false;
driveBackward(25.0);
if(irDetected == true) {
driveBackward(15.0);
raiseLift(750.0);
dropBallCenter();
wait1Msec(300);
servo[centerServo] = endPosCenter - 30;
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
driveForward(105.0);
turnRight(85.0);
driveBackward(300.0);
}
else {
irPos1 = true;
driveBackward(110.0);
turnRight(46.0);
driveBackward(135.0);
turnRight(3.0);
raiseLift(750.0);
dropBallCenter();
wait1Msec(300);
servo[centerServo] = endPosCenter - 30;
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
driveForward(130.0);
turnRight(82.0);
driveBackward(300.0);
}
}
else {
driveForward(115.0);
// ir loop
if(irDetected == true) {
irPos3 = true;
driveBackward(15.0);
raiseLift(750.0);
dropBallCenter();
wait1Msec(300);
servo[centerServo] = endPosCenter - 30;
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
driveForward(120.0);
turnRight(85.0);
driveBackward(300.0, 60);
/*turnRight(43.0);
driveBackward(250.0);
motor[rightWheel] = -20;
motor[leftWheel] = -20;
wait1Msec(200);
motor[rightWheel] = 0;
motor[leftWheel] = 0;
dropClamp();
driveForward(10.0);
raiseLift(700.0);
dropBallGoal();
resetDropGoal();*/
break;
}
else {
driveBackward(220.0);
turnRight(85.0);
driveBackward(100.0);
}
}
break;
}
}
int main(void) {
bt_init(BAUD_RATE);
Sonar_init(CONTINUOUS);
Servo_init(MANUAL, SCAN_AND_LOCK);
motorDriverInit();
while(1){
bt_getStr( tab ); // Get string from buffer
if(strlen( tab )){ // If isn't empty...
//bt_sendStr( tab ); // ...send it back.
if ( strcmp(tab, "w") == 0 ) {
driveForward(speed);
}
else if ( strcmp(tab, "s") == 0) {
driveReverse(speed);
}
else if ( strcmp(tab, " ") == 0) {
stop();
}
else if ( strcmp(tab, "a") == 0) {
driveReverseLeftTrack(40);
driveForwardRightTrack(40);
}
else if ( strcmp(tab, "d") == 0) {
driveForwardLeftTrack(40);
driveReverseRightTrack(40);
}
else if ( strcmp(tab, "ServoScanAndLock") == 0) {
ServoChangeMode(SWEEP);
ServoChangeSweepMode(SCAN_AND_LOCK);
}
else if ( strcmp(tab, "ServoScanAndGo") == 0) {
ServoChangeMode(SWEEP);
ServoChangeSweepMode(SCAN_AND_GO);
}
else if ( strcmp(tab, "ServoCenter") == 0) {
ServoChangeMode(MANUAL);
ServoMoveByDegree(0);
}
else if ( strncmp(tab, "SonarStartMeas",14) == 0) {
int new_deg = ParseIntNumber(tab,14,3);
SonarStartMeas(new_deg);
}
else if ( strncmp(tab, "SonarGetDistance",16) == 0) {
char buffor[12];
int new_deg = ParseIntNumber(tab,16,3);
sprintf(buffor, "%04d,%04hu\n",new_deg,SonarGetDistance(new_deg));
bt_sendStr(buffor);
}
else if ( strcmp(tab, "e") == 0) {
if (speed<=90){
speed+=10;
}
}
else if ( strcmp(tab, "q") == 0) {
if (speed>=10){
speed-=10;
}
}
else if (strncmp(tab, "speed",5) == 0){
int new_speed = ParseIntNumber(tab,5,3);
if (new_speed >= 0 && new_speed <= 100){
speed = new_speed;
}
}
else if ( strncmp(tab, "SonarLockRange",14) == 0) {
int new_range = ParseIntNumber(tab,14,3);
ServoChangeLockRange(new_range);
}
}
}
};
task autonomous()/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
{
int programmingSkills=0;
int match=0;
int red=0;
int blue=0;
int cube=0;
int skyrise=0;
SensorValue[leftLifttEncoder] = 0;
SensorValue[rightLiftEncoder] = 0;
if(SensorValue[autoPot2] > 1000) // competition or match
{
programmingSkills=1;
}
else
match=1;
if(SensorValue[autoPot] < 2500) //red or blue
{
red=1;
}
else
blue=1;
if(SensorValue[autoPot3] > 2000) // skyrise or cubes
{
skyrise=1;
}
else
cube=1;
// DriveLoop(-1, 800, 0.11, 0.000000, 0.001);
// DriveLoop( 1, 800, 0.11, 0.000000, 0.001);
if(match==1 && skyrise==1 && red==1)//------------------RED SKYRISE-------
{
SensorValue[latch] = 1;
armDown(0, 90);
wait1Msec(300);
armUp(130, 90);
autoFeed(300, -127);
autoFeed(1,0);
wait1Msec(500);
armDown(105, 90);
autoGrab(1);
armUp(300, 90);
liftMotors(-18);
DriveLoop(-1,-1, 890, 0.15, 0.00000001, 0.1, 120, 30);
armDown(0, 90);
autoGrab(0); //drop one
armUp(160, 90);
liftMotors(-10);
DriveLoop(1,1, 890, 0.15, 0.00000001, 0.1, 120, 30);
armDown(105, 90);
autoGrab(1);
armUp(200, 90);
DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30);
armDown(220, 90);
autoGrab(0); //drop two
armUp(290, 90);
liftMotors(-15);
DriveLoop(1,1, 900, 0.15, 0.00000001, 0.1, 120, 30);
armDown(105, 90);
autoGrab(1);
armUp(390, 90);
DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30);
armDown(320, 90);
autoGrab(0); //drop three
armUp(400, 90);
Feed(-40);
driveForward(127, -60, 160);
Feed(-127);
wait1Msec(1000);
Feed(0);
wait1Msec(5000000);
}
if(match==1 && skyrise==1 && blue==1)//---BLUE SKYRISE-----
{
SensorValue[latch] = 1;
wait1Msec(300);
armUp(160, 90);
autoFeed(300, -127);
autoFeed(1,0);
wait1Msec(500);
armDown(80, 90);
autoGrab(1);
armUp(185, 90);
liftMotors(-20);
DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30);
armDown(0, 90);
autoGrab(0); //drop one
armUp(180, 90);
liftMotors(-10);
DriveLoop(1,1, 890, 0.15, 0.00000001, 0.1, 120, 30);
armDown(80, 90);
autoGrab(1);
armUp(220, 90);
DriveLoop(-1,-1, 925, 0.15, 0.00000001, 0.1, 120, 30);
armDown(180, 90);
autoGrab(0); //drop two
armUp(250, 90);
liftMotors(-15);
DriveLoop(1,1, 925, 0.15, 0.00000001, 0.1, 120, 30);
armDown(80, 90);
autoGrab(1);
armUp(340, 90);
DriveLoop(-1,-1, 920, 0.15, 0.00000001, 0.1, 120, 30);
armDown(320, 90);
autoGrab(0); //drop three
armUp(400, 90);
driveForward(-60, 120, 100);
Feed(-127);
driveForward(-60, 120, 170);
wait1Msec(5000);
Feed(0);
wait1Msec(5000000);
}
if(programmingSkills==1) //--------------------------------SKILLS-------------
{
SensorValue[latch] = 1;
armDown(0, 90);
wait1Msec(300);
armUp(130, 90);
autoFeed(300, -127);
autoFeed(1,0);
wait1Msec(500);
armDown(90, 90);
autoGrab(1);
armUp(300, 90);
liftMotors(-15);
DriveLoop(-1,-1, 890, 0.15, 0.00000001, 0.1, 120, 30);
armDown(0, 90);
autoGrab(0); //drop one
armUp(160, 90);
liftMotors(-10);
DriveLoop(1,1, 890, 0.15, 0.00000001, 0.1, 120, 30);
armDown(90, 90);
autoGrab(1);
armUp(200, 90);
DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30);
armDown(220, 90);
autoGrab(0); //drop two
armUp(290, 90);
liftMotors(-10);
DriveLoop(1,1, 900, 0.15, 0.00000001, 0.1, 120, 30);
armDown(90, 90);
autoGrab(1);
armUp(380, 90);
DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30);
armDown(320, 90);
autoGrab(0); //drop three
armUp(370, 90);
liftMotors(-15);
DriveLoop(1,1, 905, 0.12, 0.00000001, 0.1, 80, 30);
armDown(90, 90);
autoGrab(1);
armUp(560, 90);
DriveLoop(-1,-1, 915, 0.12, 0.00000001, 0.1, 80, 30);
armDown(530, 90);
autoGrab(0); //drop four
armUp(610, 90);
liftMotors(-15);
DriveLoop(1,1, 915, 0.12, 0.00000001, 0.1, 80, 30);
armDown(90, 90);
autoGrab(1);
armUp(750, 90);
DriveLoop(-1,-1, 925, 0.12, 0.00000001, 0.1, 80, 30);
armDown(700, 90);
autoGrab(0); //drop five
armUp(750, 90);
liftMotors(-15);
DriveLoop(1,1, 950, 0.12, 0.00000001, 0.1, 80, 30);
armDown(90, 90);
autoGrab(1);
armUp(950, 90);
DriveLoop(-1,-1, 950, 0.11, 0.00000001, 0.1, 80, 30);
armDown(900, 90);
autoGrab(0); //drop six
armUp(950, 90);
liftMotors(-15);
DriveLoop(1,1, 950, 0.11, 0.00000001, 0.1, 80, 30);
armDown(90, 90);
autoGrab(1);
armUp(1220, 90);
DriveLoop(-1,-1, 900, 0.12, 0.00000001, 0.1, 80, 30);
armDown(1110, 90);
autoGrab(0); //drop seven
armUp(1220, 90);
/*liftMotors(100);
wait1Msec(500);
liftMotors(15);*/
driveForward(70, -70, 225);
driveForward(70, 70, 100);
autoFeed(750, -127);
SensorValue[conveyer] = 1;
driveForward(70, -70, 825);
armDown(20, 127);
liftMotors(-100);
wait1Msec(500);
liftMotors(-15);
driveForward(70, 70, 100);
autoFeed(1000, 127);
driveForward(70, -70, 225);
driveForward(70, 70, 100);
autoFeed(1000, 127);
driveForward(70, -70, 1000);
armUp(1220, 127);
driveForward(70, 70, 225);
autoFeed(750, -127);
wait1Msec(500);
driveForward(70, 70, 50);
autoFeed(750, -127);
driveForward(-70, -70, 200);
/*DriveLoop(1,-1, 250, 0.17, 0.00000001, 0.05, 120, 43);
DriveLoop(1,1, 140, 0.14, 0.00000001, 0.05, 80, 35);
autoFeed(1500, -127);
DriveLoop(-1,-1, 140, 0.14, 0.00000001, 0.05, 80, 35);
liftMotors(-50);
DriveLoop(-1,1, 350, 0.17, 0.00000001, 0.05, 120, 43);
liftMotors(-15);
DriveLoop(1,1, 825, 0.12, 0.00000001, 0.05, 80, 35);
DriveLoop(-1,1, 410, 0.12, 0.00000001, 0.05, 120, 43);
liftMotors(-127);
wait1Msec(1250);
liftMotors(-15);
DriveLoop(1,1, 175, 0.12, 0.00000001, 0.05, 80, 35);
autoFeed(750, 127);
wait1Msec(5000000);*/
}
if(match==1 && cube==1 && red==1) //-----------RED CUBES Match
{
DriveLoop(-1,1, 400, 0.12, 0.00000001, 0.05, 120, 50);
DriveLoop( 1,-1, 400, 0.12, 0.00000001, 0.05, 120, 50);
wait1Msec(50000000000);
/*SensorValue[latch] = 1;
driveStop(300);
armUp(1550, 90);
liftMotors(-10);
autoFeed(600, -127);
autoFeed(1,0);
wait1Msec(50);
armDown(1200, 120);
liftMotors(-127);
wait1Msec(250);
liftMotors(-15);
driveForward(100, 100, 425);
autoFeed(900, 127); //feed in red1
stopRobot(10);
liftMotors(60);
driveForward(100, 100, 350);
armUp(2300, 90);
driveForward(90, -60, 250);
autoFeed(800, -127); //feed out red1
autoFeed(1,0);
driveForward(-90, 60, 420);
armDown(1200, 110);
driveForward(100, 100, 100);
wait1Msec(50);
driveForward(70, -70, 125);
liftMotors(-127);
wait1Msec(450);
liftMotors(-15);
driveForward(100, 100, 650);
autoFeed(700, 127); //pick up blue
autoFeed(1, 0);
driveForward(-70, 70, 15);
autoFeed(375, 127); //move blue
autoFeed(1, 0);
driveForward(100, 100, 530);
autoFeed(600, 127); //pick up red2
driveForward(-70, 70, 10);
autoFeed(1, 0);
armUp(1800, 90);
driveForward(90, -60, 250);
wait1Msec(75);
autoFeed(550, -127); //drop red2
autoFeed(1,0);
driveBack(-90, -90, -100);
driveForward(-70, 70, 420);
armDown(1390, 90);
liftMotors(-10);
driveForward(90, 90, 400);*/
}
if(match==1 && cube==1 && blue==1) //-----------BLUE CUBES-------------
{
SensorValue[latch] = 1;
wait1Msec(300);
armUp(1550, 90);
liftMotors(-10);
autoFeed(600, -127);
autoFeed(1,0);
wait1Msec(50);
armDown(1425, 120);
liftMotors(-127);
wait1Msec(100);
liftMotors(-15);
SensorValue[conveyer] = 1;
driveForward(100, 100, 425);
autoFeed(925, 127); //feed in blue1
stopRobot(10);
liftMotors(40);
driveForward(100, 100, 415);
armUp(2300, 90);
driveForward(-90, 60, 265);
// driveForward(90, 90, 20);
autoFeed(800, -127); //feed out blue1
autoFeed(1,0);
// driveBack(-90, -90, -20);
driveForward(90, -60, 430);
liftMotors(-40);
driveForward(100, 100, 200);
driveForward(-70, 70, 90);
armDown(1425, 120);
liftMotors(-127);
wait1Msec(150);
liftMotors(-15);
driveForward(100, 100, 470);
autoFeed(850, 127); //pick up red
autoFeed(1, 0);
// driveForward(-70, 70, 15);
autoFeed(250, 127); //move red
autoFeed(1, 0);
driveForward(100, 100, 590);
autoFeed(650, 127); //pick up blue2
// driveForward(-70, 70, 10);
autoFeed(1, 0);
armUp(1800, 90);
driveForward(-90, 60, 290);
driveForward(90, 90, 40);
autoFeed(550, -127); //drop blue2
autoFeed(1,0);
driveBack(-90, -90, -100);
driveForward(-70, 70, 420);
armDown(1390, 90);
liftMotors(-10);
driveForward(90, 90, 400);
SensorValue[latch] = 1;
wait1Msec(300);
armUp(1550, 90);
liftMotors(-10);
autoFeed(600, -127);
autoFeed(1,0);
wait1Msec(50);
armDown(1200, 120);
liftMotors(-127);
wait1Msec(250);
liftMotors(-15);
driveForward(100, 100, 425);
autoFeed(900, 127); //feed in red1
stopRobot(10);
liftMotors(60);
driveForward(100, 100, 350);
armUp(2300, 90);
driveForward(-90, 60, 250);
autoFeed(800, -127); //feed out red1
autoFeed(1,0);
driveForward(90, -60, 420);
armDown(1450, 110);
driveForward(100, 100, 100);
wait1Msec(50);
driveForward(-70, 70, 85);
armDown(1200, 110);
liftMotors(-127);
wait1Msec(450);
liftMotors(-15);
driveForward(100, 100, 650);
autoFeed(700, 127); //pick up blue
autoFeed(1, 0);
// driveForward(-70, 70, 15);
autoFeed(375, 127); //move blue
autoFeed(1, 0);
driveForward(100, 100, 530);
autoFeed(600, 127); //pick up red2
// driveForward(-70, 70, 10);
autoFeed(1, 0);
armUp(1800, 90);
driveForward(-90, 60, 260);
wait1Msec(75);
autoFeed(550, -127); //drop red2
autoFeed(1,0);
driveBack(-90, -90, -100);
driveForward(-70, 70, 420);
armDown(1390, 90);
liftMotors(-10);
driveForward(90, 90, 400);
}
}
void play() {
HANDLE waitHandles[5] = { stopSignal, osStatusDongle.hEvent, calibratingSignal, newImageAnalyzed };
int currentx = 320, currenty = 0, FPS2Count = 0;
int timeOut = 30;
float movingTime; //timeout after sending each command
objectCollection goals, goals2;
float nearestBallFloorX = 0, nearestBallFloorY = 0, initialBallDistance = 0;
ignoreX = 0, ignoreY = 0;
Timer ignoreTimer, ballSearchRotationSpeedTimer, chargingTimer, wanderingTimer, frameTimer;
float lastBallFoundTime = 0, lastGoalFoundTime = 0, wanderingTime = 0;
bool rotateFast = true;
FPS2Timer.start();
FPS2Count = 0;
frameTimer.start();
bool turnFast = true;
state = lookForBall;
SetWindowText(goalGuessState, L"Goal on right");
isBallInDribbler = false;
prints(L"Started, driving to ball\n");
SetWindowText(stateStatusGUI, L"Driving to ball");
charged = false, ignoreBall = false;
charge();
chargingTimer.start();
sendString(hCOMDongle, "9:bl\n");
lastBallFoundTimer.start();
lastGoalFoundTimer.start();
movingTimer.start();
while (true) {
//display test stuff
float floorX, floorY;
int currentx, currenty;
findNearestFloorBall(floorX, floorY, currentx, currenty);
float angle = atanf(floorY / floorX) * 180.0 / PI;
swprintf_s(buffer, L"NX %.2f \n NY %.2f\n ang %.2f", floorX, floorY, angle);
SetWindowText(infoGUI, buffer);
//end display test stuff
switch (WaitForMultipleObjects(4, waitHandles, FALSE, timeOut)) {
//case WAIT_OBJECT_0: //start of a new command from the radio detected
// //ResetEvent(osStatus.hEvent);
// prints(L"radio event %X\n", dwCommEvent);
// //receiveCommand(); //receive and interpret the command
// //WaitCommEvent(hCOMRadio, &dwCommEvent, &osStatus); //listen to new events, ie beginning character
// continue;
case WAIT_OBJECT_0: //stop signal
prints(L"Stop signal arrived.\n");
SetWindowText(stateStatusGUI, L"stopped");
currentDrivingState.angle = 0, currentDrivingState.speed = 0, currentDrivingState.vx = 0,
currentDrivingState.vy = 0, currentDrivingState.angularVelocity = 0;
setSpeedAngle(currentDrivingState);
ResetEvent(startSignal);
return;
case WAIT_OBJECT_0 + 1: //info from the main board controller
//prints(L"main board COM event %X\n", dwCommEvent);
handleMainBoardCommunication();
WaitCommEvent(hCOMDongle, &dwCommEventDongle, &osStatusDongle); //listen to new events, ie beginning character
continue;
case WAIT_OBJECT_0 + 2: //calibrating signal
SetWindowText(stateStatusGUI, L"calibrating");
rotateAroundCenter(0);
//WaitForSingleObject(calibratingEndSignal, INFINITE);
continue;
case WAIT_OBJECT_0 + 3: //new image analyzed
//update FPS rate after every 5 images
++FPS2Count;
if (FPS2Count % 5 == 0) {
float FPS = 5.0 / FPS2Timer.time();
swprintf_s(buffer, L"FPS2: %.2f", FPS);
SetWindowText(statusFPS2, buffer);
FPS2Timer.start();
}
if (FPS2Count % 10 == 0) {
receiveCommand();
}
break;
default:
continue;
}
if (attackBlue) { //set the side
goals = goalsBlueShare;
goals2 = goalsYellowShare;
}
else {
goals = goalsYellowShare;
goals2 = goalsBlueShare;
}
if (ignoreBall) { //ignore the ball just kicked for 1s
if (ignoreTimer.time() > 1.0) {
ignoreBall = false;
ignoreX = 0, ignoreY = 0;
}
else {
float ignoreXNew = ignoreX, ignoreYNew = ignoreY;
findNearestFloorObjectToOldObject(ignoreXNew, ignoreYNew, ballsShare);
//if we cant see the ball behind the dribbler, don't start ignoring a ball too far away
ignoreX = ignoreXNew;
ignoreY = ignoreYNew;
swprintf_s(buffer, L"IgX %.2f IgY %.2f\n NX %.2f NY %.2f", ignoreX, ignoreY, ignoreXNew, ignoreYNew);
SetWindowText(infoGUI2, buffer);
}
}
else {
SetWindowText(infoGUI2, L"Ignore OFF");
}
//keep track of the likely direction to turn to to find the goal
if ((goalsBlueShare.count > 0 || goalsYellowShare.count > 0) && !(goalsBlueShare.count == 1 && goalsYellowShare.count == 1)) {
int goalX, goalY;
State stateGoalNew;
if (goals.count > 0) {
findLargestObject(goalX, goalY, goals);
stateGoalNew = goalX > 320 ? goalOnRight : goalOnLeft;
}
else if (goals2.count > 0) {
findLargestObject(goalX, goalY, goals2);
stateGoalNew = goalX > 320 ? goalOnLeft : goalOnRight;
}
if (stateGoalNew != stateGoal && state != lookForGoal) {
stateGoal = stateGoalNew;
if (stateGoal == goalOnRight) {
SetWindowText(goalGuessState, L"Goal on the right");
}
else {
SetWindowText(goalGuessState, L"Goal on the left");
}
}
}
if (ballsOnFieldInSight() > 0) { //keep track of the likely direction to turn to to find the ball
for (int i = 0; i < ballsShare.count; ++i) {
if (!ballsShare.data[i].isObjectAcrossLine) {
if (ballsShare.data[i].x > 320) {
stateBallSeen = ballOnRight;
}
else {
stateBallSeen = ballOnLeft;
}
}
}
}
float frameTime = frameTimer.time();
frameTimer.start();
if (ballsOnFieldInSight() == 0 && state == lookForBall && !isBallInDribbler) {
lastBallFoundTime += frameTime;
}
else if (ballsOnFieldInSight > 0) {
lastBallFoundTime = 0;
}
if (goals.count == 0 && state == lookForGoal) {
lastGoalFoundTime += frameTime;
}
else if (goals.count > 0) {
lastGoalFoundTime = 0;
}
if (lastBallFoundTime > 3.0 || lastGoalFoundTime > 3.0) {
prints(L"Wandering for ball\n");
state = wanderForBall;
movingTimer.start();
wanderingTimer.start();
wanderingTime = 0;
lastBallFoundTime = 0;
lastGoalFoundTime = 0;
}
if (fieldGreenPixelCountShare < FIELDGREENCOUNTTHRESHOLD && state != rotate90 && state != wanderForBall) {
state = rotate90;
movingTimer.start();
}
if (state == lookForBall) {
if (isBallInDribbler) {
prints(L"Looking for goal\n");
state = lookForGoal;
movingTimer.start();
}
else {
//prints(L"Looking for ball\n");
SetWindowText(stateStatusGUI, L"Looking for ball");
nearestBallFloorX = 0;
int sign = stateBallSeen == ballOnRight ? -1 : 1;
if (ballsOnFieldInSight() == 0) {
if (fmodf(movingTimer.time(), 0.3) < 0.2) {
rotateAroundCenter(sign * 200);
}
else {
rotateAroundCenter(sign * 40);
}
}
else {
findNearestFloorBall(nearestBallFloorX, nearestBallFloorY, currentx, currenty);
initialBallDistance = pow(nearestBallFloorX*nearestBallFloorX + nearestBallFloorY*nearestBallFloorY, 0.5);
//prints(L"found nearest ball X: %.2f, Y: %.2f, currentx: %d, currenty: %d\n", nearestBallFloorX, nearestBallFloorY, currentx, currenty);
prints(L"Driving to ball\n");
state = driveToBall;
rotateAroundCenter(0);
movingTimer.start();
}
}
}
if (state == driveToBall) {
if (isBallInDribbler) {
rotateAroundCenter(0);
Sleep(0);
prints(L"Looking for goal\n");
state = lookForGoal;
movingTimer.start();
}
else {
//prints(L"Driving to ball\n");
SetWindowText(stateStatusGUI, L"driving to ball");
float floorX, floorY;
//findNearestObject(currentx, currenty, ballsShare);
//prints(L"Floor coordinates x: %.2f, y: %.2f, currentx: %d, currenty: %d\n", floorX, floorY, currentx, currenty);
if (findNearestFloorBall(floorX, floorY, currentx, currenty) == 0) {
//prints(L"Drive state, but ballcount zero, starting to look for ball.\n");
state = lookForBall;
movingTimer.start();
prints(L"Looking for ball\n");
continue;
}
driveToFloorXYPID(floorX, floorY, initialBallDistance);
}
}
if (state == lookForGoal) {
//prints(L"Looking for goal\n");
SetWindowText(stateStatusGUI, L"Looking for goal\n");
if (isBallInDribbler) {
int x = 0, y = 0;
float floorX, floorY;
findLargestObject(x, y, goals);
convertToFloorCoordinates(x, y, floorX, floorY, GOALMIDHEIGHT);
//prints(L"Goal x: %d\n", x);
if (!(goalsBlueShare.count == 1 && goalsYellowShare.count == 1) &&
(y > 0 && floorX > 0 && fabs(floorY) < 18.0 / 100.0 || abs(x-320) <= 35)) {
state = kickBall;
rotateAroundFront(0);
prints(L"Kicking\n");
}
else {
int sign;
if (stateGoal == goalOnRight) {
sign = -1;
}
else {
sign = 1;
}
if (goals.count == 0) { //no goals in sight, turn faster
rotateAroundFront(sign * 200);
}
else{
//around 120 degs/s is max speed so that the goal doesn't go out, less than 30 degs/s is pointless
float angle = atanf(tanf(angleOfView)*(x - 320.0) / 320.0); //roughly, in degrees
float speedMultiplier = 1 - expf(-fabs(pow((x - 320.0) / 320.0, 4)));
speedMultiplier = speedMultiplier > 1 ? 1 : speedMultiplier;
float turningSpeed = 50 + (160 - 50)*speedMultiplier;
rotateAroundFront(sign * turningSpeed);
}
}
}
else {
state = lookForBall;
movingTimer.start();
prints(L"Looking for ball\n");
}
}
if (state == kickBall) {
SetWindowText(stateStatusGUI, L"Kicking");
if (!isBallInDribbler) {
state = lookForBall;
movingTimer.start();
}
if (!charged) {//not charged, wait more
if (chargingTimer.time() > 3.5) { //by this time we definitely should have gotten a charge command, charge again
prints(L"No charge info received in time\n");
chargingTimer.start();
charge();
}
continue;
}
kick();
Sleep(20);
charged = false;
//isBallInDribbler = false;
//sendString(hCOMDongle, "9:bl\n");
ignoreBall = true;
ignoreX = 25.0 / 100.0, ignoreY = 0;
ignoreTimer.start();
state = lookForBall;
movingTimer.start();
prints(L"Looking for ball\n");
//Sleep(80);
charge();
chargingTimer.start();
}
if (state == rotate90) {
SetWindowText(stateStatusGUI, L"Rotating 90");
if (movingTimer.time() < 0.5) {
rotateAroundCenter(180);
continue;
}
else {
prints(L"Looking for ball\n");
stateBallSeen = ballOnLeft;
stateGoal = goalOnLeft;
state = lookForBall;
movingTimer.start();
}
}
//wander around, try to drive towards a goal far enough away, if can't find one, drive to a direction where there are no goals, lines and there is enough green
if (state == wanderForBall) {
wanderingTime += frameTime;
SetWindowText(stateStatusGUI, L"Wandering");
if (wanderingTime > 4.0) { //if we didn't find a far away goal in 4 seconds
int goalBlueX, goalBlueY;
int goalYellowX, goalYellowY;
findLargestObject(goalBlueX, goalBlueY, goalsBlueShare);
findLargestObject(goalYellowX, goalYellowY, goalsYellowShare);
if (goalsBlueShare.count != 0 && goalBlueX >= 150 && goalBlueX <= 500
|| goalsYellowShare.count != 0 && goalYellowX >= 150 && goalYellowX <= 500 || isLineStraightAhead || fieldGreenPixelCountShare < FIELDGREENCOUNTTHRESHOLD) {
rotateAroundCenter(100);
wanderingTimer.start();
}
else {
if (wanderingTimer.time() > 1.0) {
movingTimer.start();
state = lookForBall;
prints(L"Looking for ball\n");
continue;
}
driveForward(0.5);
}
}
else {
if (goalsBlueShare.count == 0 && goalsYellowShare.count == 0 || isLineStraightAhead || fieldGreenPixelCountShare < FIELDGREENCOUNTTHRESHOLD) {
rotateAroundCenter(120);
wanderingTimer.start();
}
else {
if (wanderingTimer.time() > 1.0) {
movingTimer.start();
state = lookForBall;
prints(L"Looking for ball\n");
continue;
}
int goalX = 0, goalY = 0;
findLargestObject(goalX, goalY, goalsBlueShare);
if (goalY > 450) {
driveForward(0.5);
continue;
}
else {
goalX = 0, goalY = 0;
findLargestObject(goalX, goalY, goalsYellowShare);
if (goalY > 450) {
driveForward(0.5);
}
else {
rotateAroundCenter(100);
wanderingTimer.start();
}
}
}
}
}
}
}
void rejectionBlue()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1700;
int pot = analogRead(8);
int midLine = analogRead(2);
int encoder1 = 1800; //drive under wall
int encoder2 = 130; // rotate 90 degrees
int encoder3 = 1400; // backwards to the opponets goal
int encoder4 = 100; //turn to goal + enclose it
int encoder5 = 450; //hit 1st blue ball off
int encoder6 = 350; // drive back
int encoder7 = 175; // turn to 2nd blue ball
int encoder8 = 550; // hit 2nd ball off
int encoder9 = 250; // drive back to position
int encoder10 = 370; // rotate to line
// begin routine
intakeDead(); // unfold
delay(300); // needs to clear the wall..
driveBack(encoder1); // drive backwards to under the bridge
stopIntake();
turnLeft(encoder2); // turn ass to opponets goal
driveBackDead(encoder3); // drive to opponets goal
delay(500);
midLine = 3000;
while (midLine > 2000)
{
midLine = analogRead(2);
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, 10); // no inertia
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 10);
motorSet(MOTOR_ARM_LEFT_BACK, 10);
motorSet(MOTOR_ARM_LEFT_FRONT, 10);
delay(85);
armUp(wallHeight); // arm up
turnRight(encoder4); // turn towards center blue ball
driveForward(encoder5); // hit 1st ball off
delay(700);
driveBack(encoder6); // back
delay(700);
turnRight(encoder7); // turn towards 2nd blue ball
delay(700);
driveForward(encoder8); // hit off 2nd blue ball
delay(700);
intakeDead();
driveBack(encoder9); // position
delay(700);
turnLeft(encoder10); // encase opponet goal
delay(700);
armUp(goalHeight);
findLineRight();
//followLine(300);
driveForwardDead(); // drive to goal
delay(700);
stopDrive();
outtake(3000);
stopAll();
delay(60000);
}
void classic15Red()
{
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1550;
int dead1 = 1000;
int dead2 = 2000;
int dead3 = 3000;
int pot = analogRead (8);
//encoder values
int encoder1 = 1125; //drive to goal
int encoder2 = 325; //keep going towards goal
int encoder3 = 0; //drive slow, adjust to 90 degrees
int encoder4 = 275; //back up
int encoder5 = 800; //back up across the barrier again
int encoder6 = 90; // turn towards center large ball
int encoder7 = 220; // hit it off the barrier
int encoder8 = 350; // drive back
int encoder9 = 110; // turn towards other large ball
int encoder10 = 681; // hit it off the barrier
int encoder11 = 300; // drive back to square
int encoder12 = 150; // ass to wall
int encoder13 = 800;
int encoder14 = 400;
int encoder15 = 700;
int encoder16 = 400;
int encoder17 = 100;
int encoder18 = 400;
int encoder19 = 700;
int encoder20 = 400;
int encoder21 = 100;
int encoder22 = 400;
int encoder23 = 700;
// begin routine
intakeDead();
delay(1000);
stopIntake();
driveForward(encoder1); // drive to goal
armUp (goalHeight); //raise arm after cross barrier
driveForwardSlow(encoder2); //keep going towards goal
driveForwardSlowDead (dead1); //drive slow, adjust to 90 degrees
outtake (1500);
driveBack(encoder4); // back up
armDown(armMin); // lower arm
driveBack(encoder5); //back up across the barrier again
turnRight(encoder6); // turn towards center large ball
armUp(wallHeight); // arm up to wall height
driveForward(encoder7); // hit it off the barrier
driveBack(encoder8); // drive back
turnLeft(encoder9); // turn towards other large ball
driveForward(encoder10); // hit it off the barrier
driveBackSlowDead (dead2); // drive back to square
turnLeftArc (encoder12); // ass to wall
driveBackSlowDead (dead2) ; // wall align 90degrees
armDown (armMin); // arm down to floor
intakeDead (); // start intaking
driveForwardSlow (encoder13) ;// drive towards barf and intake whatever
turnRightArc (encoder14); // rotate towards the barrier
stopIntake();
armUp(wallHeight); // arm up to wall height
driveForwardSlow (encoder15); // drive to barrier
stopDrive(); // stop at barrier
outtake (1500); //drop the barf
driveBackSlowDead(dead2); //hump da bump
armDown (armMin); // arm down to floor
intakeDead (); //start intaking
turnLeftArc (encoder16); //face the barf
driveForwardSlow (encoder17) ;// drive towards barf and intake whatever
turnRightArc (encoder18) ; // rotate towards the barrier
stopIntake();
armUp(wallHeight); // arm up to wall height
driveForwardSlow (encoder19); // drive to barrier
stopDrive(); // stop at barrier
outtake (1500); //drop the barf
driveBackSlowDead(dead2); //drive to and align on bump
armDown (armMin); // arm down to floor
intakeDead (); //start intaking
turnLeftArc (encoder20); //face the barf
driveForwardSlow (encoder21) ;// drive towards barf and intake whatever
turnRightArc (encoder22) ; // rotate towards the barrier
stopIntake();
armUp(wallHeight); // arm up to wall height
driveForwardSlow (encoder23); // drive to barrier
outtake (1500); //drop the barf
//end of routine
stopAll () ;
delay(60000);///////////////////////////////////////////////////////////////////////////////////
}
// drive the robot
// -------------------------------------------------------------------[ drive ]
void WRTbotDrive::drive( const uint16_t driveThisWay, wrt_motor_t &motor )
{
// --- motor ON by default
// -----------------------
motor.left.control |= wrt_motorControl_power_ON;
motor.right.control |= wrt_motorControl_power_ON;
// --- no drive by light
// --------------------------
this->driveByLightFactor = 0;
switch( driveThisWay )
{ default:
// ------------------------------------------------------------[ stop ]
case wrt_stop:
{
motor.left.control &= wrt_motorControl_power_OFF;
motor.right.control &= wrt_motorControl_power_OFF;
// ---
motor.left.speed = 0;
motor.right.speed = 0;
// ---
drive( motor );
} break;
// ---------------------------------------------------------[ forward ]
case wrt_forward:
{
this->useNominal = true;
// ---
driveForward( motor );
} break;
// ---------------------------------------------------------[ forward ]
case wrt_forward_slow:
{
this->useNominal = false;
motor.right.speed = wrt_code_motor_speed_min;
motor.left.speed = wrt_code_motor_speed_min;
// ---
driveForward( motor );
} break;
// ----------------------------------------------------[ fast forward ]
case wrt_forward_fast:
{
this->useNominal = false;
motor.right.speed = wrt_code_motor_speed_max;
motor.left.speed = wrt_code_motor_speed_max;
// ---
driveForward( motor );
} break;
// ---------------------------------------------------------[ reverse ]
case wrt_reverse:
{
this->useNominal = true;
// ---
driveReverse( motor );
} break;
// ----------------------------------------------------[ fast reverse ]
case wrt_reverse_fast:
{
this->useNominal = false;
motor.right.speed = wrt_code_motor_speed_max;
motor.left.speed = wrt_code_motor_speed_max;
// ---
driveReverse( motor );
} break;
// -------------------------------------------------------[ left turn ]
case wrt_drive_left:
{
driveLeft( motor );
} break;
// --------------------------------------------------[ fast left turn ]
case wrt_turn_left_fast:
{
turnLeftFast( motor );
} break;
// --------------------------------------------------[ slow left turn ]
case wrt_turn_left:
{
this->useNominal = true;
turnLeft( motor );
} break;
// ------------------------------------------------------[ right turn ]
case wrt_drive_right:
{
driveRight( motor );
} break;
// -------------------------------------------------[ fast right turn ]
case wrt_turn_right_fast:
{
turnRightFast( motor );
} break;
// -------------------------------------------------[ slow right turn ]
case wrt_turn_right:
{
this->useNominal = true;
turnRight( motor );
} break;
// -----------------------------------------[ differential right turn ]
case wrt_turnRightDiff:
case wrt_turnRightDiff_fast:
{
turnRightDiferential( driveThisWay, motor );
} break;
// -----------------------------------------[ differential left turn ]
case wrt_turnLeftDiff:
case wrt_turnLeftDiff_fast:
{
turnLeftDiferential( driveThisWay, motor );
} break;
}
} // done drive()
void kakitRed()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1650;
int pot = analogRead(8);
int encoder00 = 250; // back up abit to row
int encoder0 = 950; // turn 360 degrees, knock off buckys, face line
int encoder1 = 162; // rotate towards 2 buckys
int encoder2 = 150; // drive towards buckys
int encoder3 = 400; // back up to bump
int encoder4 = 200; // back up towards bridge
int encoder5 = 360; // rotate 180 degrees to the large ball
int encoder6 = 900; // go under bridge and knock out large ball
int encoder7 = 90; // turn to goal
int encoder8 = 200; // drive to goal
int encoder9 = 75;
// begin routine
intake(300);
armDownTrim();
driveForwardDead(); //ram big balls
delay(1500);
stopDrive();
delay(500);
driveBack(encoder00); // back up to row abit
intakeDead();
turnLeft(encoder0); // turn 360 degrees
driveBackDead(); // drive back + wall align
delay(1300);
stopDrive();
delay(500);
turnRight(encoder1); // turn to two buckys
intakeDead();
followLine(300); // make sure ur straight
driveForwardSlowDead(); // drive towards buckys
delay(500);
stopDrive();
delay(600);
driveForwardDead(); //get the 2nd ball
delay(200);
stopDrive();
delay(600);
stopIntake();
driveBack(encoder3); // back up to bump
armUpDead(); // armup
delay(50);
stopArm();
stopIntake();
driveBackSlowDead(); // allign the bump
delay(300);
stopDrive();
delay(500);
driveBackDead(); // over the bump
delay(1000);
stopDrive();
delay(500);
driveForwardSlowDead(); // alighn to bump
delay(800);
stopDrive();
delay(500);
driveBackSlow(encoder4); // back up towards bridge
turnLeft(encoder5); // rotate 180 degrees to the large ball
armDown(armMin);
armDownTrim();
driveForward(encoder6); // go under the bridge + knock large ball
armUp(goalHeight);
turnRight(encoder7); // turn to goal
findLineRight();
followLine(300);
driveForwardSlowDead(); // drive to goal
delay(700);
stopDrive();
outtake(7000); // outtake 3
stopAll();
}
void Autonomous()
{
DriverStationLCD *screen = DriverStationLCD::GetInstance();
while ((IsAutonomous()))
{
HSLImage* imgpointer; // declares an image container as an HSL (hue-saturation-luminence) image
imgpointer = camera.GetImage(); //tells camera to capture image
backpack.Set(Relay::kForward); //turns ringlight on
BinaryImage* binIMG = NULL; // declares a container to hold a binary image
binIMG = imgpointer -> ThresholdHSL(0, 255, 0, 255, 235, 255); // thresholds HSL image and places in the binary image container
delete imgpointer; // deletes the HSL image to free up memory on the cRIO
Image* Kirby = imaqCreateImage(IMAQ_IMAGE_U8, 0); //create 8 bit image
Image* KirbyTwo = imaqCreateImage(IMAQ_IMAGE_U8, 0); // creates the second 8-bit image that we can use separately for counting particles.
// (The first image gets eaten by the measureparticle function)
float pLower = 175; // min height of rectangle for comparison
float pUpper = 500; // max height of rectangle for comparison
int criteriaCount = 1; // number of elements to include/exclude at a time
int rejectMatches = 1; // when set to true, particles that do not meet the criteria are discarded
int connectivity = 1; // declares connectivity value as 1; so corners are not ignored
int Polturgust3000; // removes small blobs
int borderSetting; // variable to store border settings, limit for rectangle
int cloningDevice; // we create another image because the ParticleMeasuring steals the image from particlecounter
int borderSize = 1; // border for the camera frame (if you don't put this, DriverStation gets mad at you)
ParticleFilterCriteria2 particleCriteria;
ParticleFilterOptions2 particleFilterOptions;
int numParticles;
particleCriteria.parameter = IMAQ_MT_AREA; //The Morphological measurement we use
particleCriteria.lower = pLower; // The lower bound of the criteria range
particleCriteria.upper = pUpper; // The upper bound of the criteria range
particleCriteria.calibrated = FALSE; // We aren't calibrating to real world measurements. We don't need this.
particleCriteria.exclude = TRUE; // Remove all particles that aren't in specific pLower and pUpper range
particleFilterOptions.rejectMatches = rejectMatches; // Set to 1 above, so images that do not meet the criteria are discarded
particleFilterOptions.rejectBorder = 0; // Set to 0 over here so border images are not discarded
particleFilterOptions.connectivity8 = connectivity; // Sets the image image to 8 bit
Polturgust3000 = imaqParticleFilter4(Kirby, binIMG -> GetImaqImage(), &particleCriteria, criteriaCount, &particleFilterOptions, NULL, &numParticles); //The Particle Filter Function we use. (The ones before it are outdated)
borderSetting = imaqSetBorderSize(Kirby, borderSize); // Sets a border size
cloningDevice = imaqDuplicate(KirbyTwo, Kirby); //Officially creating a duplicate of the first image to count the number of particles.
delete binIMG; //Deletes the Binary image
int ParticleCounter; // stores number of particles
int* countparticles; // stores the number of particles for the measure particle function
ParticleCounter = imaqCountParticles(Kirby, TRUE, countparticles); // Counts the number of particles to be sent off later to the MeasureParticle function. Then it gets eaten by the measureparticle function
int TinyRuler; // TRULY ARBITRARY name of the first measuring particle function (specifically for particle #1)
int BabyYardstick; // TRULY ARBITRARY Name of the second measuring particle function (specifically for particle #2)
double* unowidth; // TRULY ARBITRARY name of the first particle it find
double* doswidth; // TRULY ARBITRARY name of the second particle it finds
TinyRuler = imaqMeasureParticle(Kirby, 0, FALSE, IMAQ_MT_BOUNDING_RECT_WIDTH, unowidth); // Function of measuring rectangle width is applied to particle 1 (unowidth)
BabyYardstick = imaqMeasureParticle(Kirby, 1, FALSE, IMAQ_MT_BOUNDING_RECT_WIDTH, doswidth); // Function of measuring width is applied to particle 2 (doswidth)
screen->PrintfLine(DriverStationLCD::kUser_Line3,"W1: %f",*unowidth); // Prints the applied information to particle 1. (Rectangle width)
screen->PrintfLine(DriverStationLCD::kUser_Line4,"W2: %f",*doswidth);
imaqDispose(Kirby);
imaqDispose(KirbyTwo);
if (((togglebuttonOne.Get()) == 0) && ((togglebuttonTwo.Get()) == 1))
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Left Position");
if (*unowidth > 20) // The target should be hot. Now it goes to the other goal.
// Even this needs to be tested
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Left Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnRight();
driveForward();
Wait(3);
stopDriving();
shootCatapult();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Left Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
shootCatapult();
}
}
//both on
else if (((togglebuttonOne.Get()) == 1) && ((togglebuttonTwo.Get()) == 1))
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position");
if (*unowidth > 20) // The target should be hot. Now it goes to the other goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Middle Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnLeftMore();
driveForward();
Wait(3);
stopDriving();
shootCatapult();
}
else if (((togglebuttonOne.Get()) == 0) && ((togglebuttonTwo.Get()) == 0))
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position");
if (*unowidth > 20) // The target should be hot. Now it goes to the other goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Middle Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnRightMore();
driveForward();
Wait(3);
stopDriving();
shootCatapult();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Middle Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
shootCatapult();
driveForward();
Wait(3);
stopDriving();
}
}
//Left button on && right off
else if (((togglebuttonOne.Get()) == 1) && ((togglebuttonTwo.Get()) == 0))
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Right Position");
if (*unowidth > 20) // The target should be hot. Now it goes to the other goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Right Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnLeft();
driveForward();
Wait(3);
stopDriving();
shootCatapult();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Right Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
shootCatapult();
}
}
Wait(0.005);
screen -> UpdateLCD();
}
}
}
