|
Re: [FTC]: Autonomous
I don't have the teleop code, it is on a laptop that stays at my school. I worked on the autonomous code primarily. This is what ours looked like..
#pragma config(Hubs, S1, HTMotor, HTMotor, HTServo, none)
#pragma config(Sensor, S3, , sensorTouch)
#pragma config(Motor, mtr_S1_C1_1, motorD, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C1_2, motorE, tmotorNormal, openLoop)
#pragma config(Servo, srvo_S1_C3_1, ser1, tServoNormal)
#pragma config(Servo, srvo_S1_C3_2, ser2, tServoNormal)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
const tSensors touchSensor = (tSensors)S3;
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Autonomous Mode Code Template
//
// This file contains a template for simplified creation of an autonomous program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
#include "JoystickDriver.c" //Include file to "handle" the Bluetooth messages.
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// initializeRobot
//
// Prior to the start of autonomous mode, you may want to perform some initialization on your robot.
// Things that might be performed during initialization include:
// 1. Move motors and servos to a preset position.
// 2. Some sensor types take a short while to reach stable values during which time it is best that
// robot is not moving. For example, gyro sensor needs a few seconds to obtain the background
// "bias" value.
//
// In many cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
void initializeRobot()
{
// Place code here to sinitialize servos to starting positions.
// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
return;
}
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Main Task
//
// The following is the main code for the autonomous robot operation. Customize as appropriate for
// your specific robot.
//
// The types of things you might do during the autonomous phase (for the 2008-9 FTC competition)
// are:
//
// 1. Have the robot follow a line on the game field until it reaches one of the puck storage
// areas.
// 2. Load pucks into the robot from the storage bin.
// 3. Stop the robot and wait for autonomous phase to end.
//
// This simple template does nothing except play a periodic tone every few seconds.
//
// At the end of the autonomous period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
//variables
int moveforwarddone = 0; //flag for once done moving forward
short startTime; //time at start of loops
int turnleftdone = 0; //flag for once done turning left
int turnrightdone = 0; //flag for once done turning right
int liftarmdone = 0; //flag for once done lifting arm
int lowerarmdone = 0; //flag for once done lowering arm
int reversedone = 0; //flag for once done reversing robot
short pulleymotor1; //motor variable that assists servos
short pulleymotor2; //motor variable that assists servos
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
//// ////
//// Add your robot specific autonomous code here. ////
//// ////
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
while (true)
{
startTime = (short)nPgmTime; //set the program time back to 0
while(nPgmTime < startTime + 3000) //approximate distance from wall needed to turn
{
//while the sensor reads more than 13 cm. away from wall move forward full speed
motor[motorD] = 127;
motor[motorE] = -128;
moveforwarddone = 1;
}
// turn robot left
if(moveforwarddone == 1)
{
startTime = (short)nPgmTime;
while(nPgmTime < startTime + 500) // change to compass sensor -- need to get bearings once we get to competition location
{
motor[motorD] = 127; //full speed with right wheel(make sure right wheel is motor A)
motor[motorE] = 127; //full reverse left wheel (make sure the left wheel is motor B)
}
turnleftdone = 1;
moveforwarddone = 0; //reset moveforward done
}
//move robot towards the trough (3' 5.5") about 106 cm. NEED TO TEST PROBABLY HAVE TO RUN FOR TIME
if(turnleftdone == 1)
{
startTime = (short)nPgmTime; //set the program time back to 0
while(nPgmTime < startTime + 2000) //may need to lower or increase the amount of time for raising arms
{
motor[motorD] = 127; //move forward maximum speed
motor[motorE] = -128;
}
turnleftdone = 0; //reset turnleftdone
moveforwarddone = 1; //move forward done == 1 after bot reaches required position
}
//turn back to original position
if(moveforwarddone == 1)
{
startTime = (short)nPgmTime; //set the program time back to 0
while(nPgmTime < startTime + 250) // run while heading != original heading (forward)
{
motor[motorD] = -128; //full reverse with right wheel
motor[motorE] = -128; //full speed with left wheel
}
turnrightdone = 1;
moveforwarddone = 0; //reset moveforward done
}
//lift arm to necessary hight (20.75") raise for time
if(turnrightdone == 1)
{
startTime = (short)nPgmTime;
while(nPgmTime < startTime + 2000) // make pulleymotors lift to assist servos
{
motor[pulleymotor1] = 127;
motor[pulleymotor2] = 127;
}
servo[ser1] = 80; //make both servos lift the arm at maximum speed
servo[ser2] = 175;
liftarmdone = 1;
moveforwarddone = 0; //reset move forward done
}
//move robot towards the wall to get arm close enough
if(liftarmdone == 1)
{
startTime = (short)nPgmTime; //set the program time back to 0
while(nPgmTime < startTime + 250) //approximate distance from wall needed to turn
{
//while the sensor reads more than 13 cm. away from wall move forward full speed
motor[motorD] = 127;
motor[motorE] = -128;
}
liftarmdone = 0; //reset lift arm
moveforwarddone = 1;
}
//lower arm if near a wall
if(moveforwarddone == 1)
{
startTime = (short)nPgmTime;
while(nPgmTime < startTime + 2000) // make pulleymotors lower
{
motor[pulleymotor1] = -128;
motor[pulleymotor2] = -128;
}
servo[ser1] = 0; //make both servos lower the arm at maximum speed
servo[ser2] = 255;
lowerarmdone = 1;
moveforwarddone = 0; //reset move forward done
}
//turn robot so that back faces straight towards the scoring area
if(lowerarmdone == 1)
{
startTime = (short)nPgmTime; //set the program time back to 0
while(nPgmTime < startTime + 150) // run while heading != original heading (about 20 degrees)
{
motor[motorD] = -128; //full reverse with right wheel
motor[motorE] = -128; //full speed with left wheel
}
turnleftdone = 1;
lowerarmdone = 0; //reset lower arm done
}
//reverse robot to scoring area
if(turnleftdone == 1)
{
while(SensorValue[touchSensor] < 100 ) // run while touch sensor is not pushed in
{
motor[motorD] = -128; //full reverse with right wheel
motor[motorE] = 127; //full reverse with left wheel
}
turnleftdone = 0; //reset lower are done
reversedone = 1;
}
//score pucks until time runs out
if(reversedone == 1)
{
startTime = (short)nPgmTime;
while(nPgmTime < startTime + 2000) // make pulleymotors lift to assist servos
{
motor[pulleymotor1] = 127;
motor[pulleymotor2] = 127;
}
servo[ser1] = 110; //raise arm at full speed
servo[ser2] = 145;
}
}
}
|