task main()
{
bFloatConversionErrors=true;
displayTextLine(0, "HT Gyro");
displayTextLine(1, "Test task");
displayTextLine(5, "Press enter");
displayTextLine(6, "to set relative");
displayTextLine(7, "heading");
sleep(2000);
eraseDisplay();
startTask(gyro_loop);
while(true)
{
if(getXbuttonValue(xButtonEnter))
{
gyro_loop_state = CALIBRATION;
sleep(2000);
}else if(getXbuttonValue(xButtonLeft))
{
gyro_loop_state = STOPPED;
while(gyro_loop_state!=STOPPED)
sleep(2000);
stopTask(gyro_loop);
}else if(getXbuttonValue(xButtonRight))
{
startTask(gyro_loop);
}else
{
sleep(1000);
}
}
}
void autonomous3Left() {
motor[intake] = 127;
motor[indexer] = 127;
startAutoFlywheel(300, HIGH_SPEED_HOLD, LOW_SPEED_HOLD, WAIT_HOLD);
setWheelSpeed(45);
wait1Msec(1300);
setWheelSpeed(0);
wait1Msec(2000);
autonIndex = true;
autonShoot = false;
autonIntake = true;
startTask(stopFlywheel);
startTask(intakeControl);
turnPID(350);
drivePID(1000);
stopTask(intakeControl);
motor[intake] = 0;
motor[indexer] = 0;
turnPID(230);
motor[intake] = -127;
motor[indexer] = -127;
wait1Msec(1000);
turnPID(400);
wait1Msec(200);
setWheelSpeed(80,100);
wait1Msec(1000);
setWheelSpeed(0)
}
task autonomous()
{
//8.3 battery working
setUp();
startTask(FlywheelController);
startTask(recoverFromShots);
startTask(rpmIndicator);
SetTarget(2610,85);
wait1Msec(5000);
Conveyor(127);
wait1Msec(500);
Conveyor(0);
wait1Msec(2000);
Conveyor(127);
wait1Msec(500);
Conveyor(0);
wait1Msec(2000);
Conveyor(127);
wait1Msec(500);
Conveyor(0);
wait1Msec(2000);
Conveyor(127);
while(true)
{}
}
task usercontrol()
{
// startTask(shooter);
startTask(drive);
startTask(intake);
speedUpFlywheel();
while(true){
if(vexRT(Btn8U)){
motor[LUflywheel] = 127;
motor[LDflywheel] = 127;
motor[RUflywheel] = 127;
motor[RDflywheel] = 127;
}
else if(vexRT(Btn8D)){
motor[LDflywheel] = 100;
motor[RUflywheel] = 100;
motor[RDflywheel] = 100;
}
else if(vexRT(Btn8L))
slowDownFlywheel();
wait1Msec(25);
}
}
task autonomous() {
initializeTasks();
stopTask(seymoreControl);
//startTask(skillPointAuto);
//startTask(stationaryAuto);
//startTask(hoardingAuto);
//startTask(classicAuto);
//startTask(skillz);
if (SensorValue[ternaryPot] < 1187) {
if (SensorValue[binaryPot] < 1217) {
startTask(stationaryAuto);
}
else {
startTask(skillPointAuto);
}
}
else if (SensorValue[ternaryPot] < 2577) {
if (SensorValue[binaryPot] < 1217) {
startTask(classicAuto);
}
else {
startTask(hoardingAuto);
}
}
else {
startTask(skillz);
}
while(true) { EndTimeSlice(); }
}
void init() {
playTone(700,10);
startTask(LCD);
startTask(flywheelVelocity);
setBaudRate(UART1, baudRate57600);
//Slave Motors
slaveMotor(flywheel2,flywheel4);
slaveMotor(flywheel3,flywheel4);
slaveMotor(flywheel1,flywheel4);
//Startup modes
if(!debugMode)
debugMode = (bool) SensorValue[debug];
if(!tuneMode)
tuneMode = (bool) SensorValue[tune];
if(!encoderTestMode)
encoderTestMode = (bool) SensorValue[encoderTest];
//Boot into test encoder mode
if(encoderTestMode)
testEncoder();
bool autonIntake = false;
bool autonIndex = false;
bool autonShoot = false;
}
task usercontrol() {
startTask(drive);
while(true) {
if(vexRT(Btn8U)) {
shooterPowerDown();
autoFeeder = false;
}
if(vexRT(Btn8D) || autoStartShooter) {
autoFeeder = true;
if(fastMode) {
speed = 103;
feederWaitTime = 1000;
} else {
speed = 98;
feederWaitTime = 1500;
}
startTask(shooterDJ);
}
if(vexRT(Btn5D))
motor[feeder] = 127;
else if(!autoFeeder)
motor[feeder] = 0;
if(vexRT(Btn5U))
motor[intake1] = 127;
else
motor[intake1] = 0;
if(vexRT(Btn6D)) {
speed = 55;
startTask(shooter);
}
wait1Msec(25);
}
}
//for flywheel acceleration; the separate task lets the acceleration code run concurently with other robot functions
task flywheelController() { //manages flywheel starts and stops
while(1)
{
//activate/deactivate flywheel on joystick button press
if(vexRT[Btn5D] == 1 && !flywheelRunning){
leftFwMotorSet(40);
rightFwMotorSet(40);
wait1Msec(750);
leftFwMotorSet(70);
rightFwMotorSet(70);
wait1Msec(750);
leftFwMotorSet(75);
rightFwMotorSet(75);
wait1Msec(500);
startTask(leftFwControlTask); //this is ok to run every time because stopping the flywheel also stops the flywheel control tasks
startTask(rightFwControlTask);
leftFwVelocitySet(74,0.59);//was 100
rightFwVelocitySet(74,0.59);//was 100
startTask(flashYellowLED);
}
else if (vexRT[Btn7D] == 1 && vexRT[Btn8D] == 1) {
stopTask(leftFwControlTask);
stopTask(rightFwControlTask);
leftFwMotorSet(0);
rightFwMotorSet(0);
stopTask(flashYellowLED);
SensorValue[yellowLED] = 0; //make sure LEDs are off
SensorValue[redLED] = 1;
wait1Msec(3000);
SensorValue[redLED] = 0;
}
}
}
task usercontrol()
{
bool clicked;
setUp();
startTask(driverControl);
startTask(conveyorControl);
startTask(targetAdjustment);
driverTarget = 2610;
SetTarget(driverTarget, 85);
while(true)
{
while(vexRT[Btn8D] == 0)
{
if(vexRT[Btn8L] == 1)
{
while(vexRT[Btn8L] == 1)
{
wait1Msec(10);
}
driverTarget = 2610;
SetTarget(driverTarget, 85);
}
if(vexRT[Btn8U] == 1)
{
while(vexRT[Btn8U] == 1)
{
wait1Msec(10);
}
driverTarget = 1800;
SetTarget(driverTarget, 50);
}
if(vexRT[Btn8R] == 1)
{
while(vexRT[Btn8R] == 1)
{
wait1Msec(10);
}
driverTarget = 1000;
SetTarget(driverTarget, 30);
}
wait1Msec(10);
}
while(vexRT[Btn8D] == 1)
{
wait1Msec(10);
}
clicked = !clicked;
if(clicked)
{
startTask(FlywheelController);
startTask(recoverFromShots);
SetTarget(driverTarget, 85);
}
else
{
SetTarget(0, 0);
}
}
}
task autonomous()
{
wait1Msec(5000);
startTask(FlywheelController);
SetTarget(1600,80);
startTask(recoverFromShots);
wait1Msec(4500);
SensorValue[ANGLE] = 0;
clearTimer(T3);
while(time1[T3] <2550)
{
motor[rdy] = motor[rd] = -127 - (SensorValue[ANGLE]); // was 8
motor[ld] = motor[ldy] = -127 + (SensorValue[ANGLE]);
wait1Msec(10);
}
motor[rd] = motor[rdy] = 0;
motor[ld] = motor[ldy] = 0;
startTask(autoConveyor);
while(true)
{}
// AutonomousCodePlaceholderForTesting(); // Remove this function call once you have "real" code.
}
task usercontrol()
{
startTask(shooter);
startTask(drive);
startTask(intake);
startTask(flywheelVelocity);
//speedUpFlywheel();
while(true){
if(vexRT(Btn8U)){
motor[LUflywheel] = 127;
motor[LDflywheel] = 127;
motor[RUflywheel] = 127;
motor[RDflywheel] = 127;
}
else if(vexRT(Btn8D)){
motor[LDflywheel] = 90;
motor[RUflywheel] = 90;
motor[RDflywheel] = 90;
}
if(vexRT(Btn8L)){
stopTask(flywheelP);
slowDownFlywheel();
}
wait1Msec(25);
}
}
task main() {
while(true) {
startTask(initialize);
startTask(gyroTest);
}
}
task rij_auto()
{
startTask(ramp_up_auto);
startTask(sound);
startTask(scancode);
wait10Msec(10);
float speedL = 0, speedR = 0, Grayscale = 0;
while(true)
{
Grayscale = SensorValue[LichtR];
if (error == 0 || (perverror < 0 && error > 0) || (perverror > 0 && error < 0))
{
integral = 0;
}
error = Grayscale - offset; // berekent de error value (hoever de sensor van de lijn zit.)
integral = (2/3)*integral + error; //opsomming van de integrals
derivative = error - perverror;
//datalogAddValue(derivative, derivative);
turn = (Kp * error) + (Ki * integral) + (Kd * derivative); // berekening waarbij bepaald word hoeveel er bijgestuurd moet worden om op de lijn te blijven.
speedL = (Tp - turn);
speedR = (Tp + turn);
//displayString(6, "R= %d, L= %d", speedR, speedL);
motor[MotorLinks] = speedL;
motor[MotorRechts] = speedR;
perverror = error;
//lightrr = SensorValue[LichtR]; //debug
//lightll = SensorValue[LichtL]; //debug
}
}
task autonomous() {
//start flywheel
initializeTasks();
setFlywheelRange(2);
wait1Msec(1000);
startTask(fire);
//wait until first set of preloads are fired
waitUntilNotFiring(15000);
while (firing) { EndTimeSlice(); }
stopTask(fire);
turn(120); //turn toward stack
motor[feedMe] = 127;
driveStraight(150, 1, 1, 60); //move to second stack
turn(-30);
driveStraight(3200, 1, 1, 60); //drive across field
autonProgress = 1;
turn(-83); // turn toward net
autonProgress = 2;
//fire remaining balls
startTask(fire);
while (true) { EndTimeSlice(); }
}
//prepare to use TBH for flywheel velocity control
void initializeTBH() {
tbhInit(lFly, 392, 0.0004);//.0002 //initialize TBH for left side of the flywheel
tbhInit(rFly, 392, 0.0004); //.000275 //initialize TBH for right side of the flywheel
//motor[intake] = 127;
//start the flywheel control tasks
startTask(leftFwControlTask);
startTask(rightFwControlTask);
}
//purple shooting (for skills)
void initializePIDPurple() {
//note the order of the parameters:
//(controller, motor ticks per rev, KpNorm, KpBallLaunch, Ki, Kd, constant, RPM drop on ball launch)
tbhInit(lFly, 392, 0.4281, 3.03, 0.005481, 0, 55, 20); //initialize PID for left side of the flywheel //left side might be able to have a higher P
tbhInit(rFly, 392, 0.4281, 3.03, 0.005481, 0, 55, 20); //initialize PID for right side of the flywheel //x.x481
startTask(leftFwControlTask);
startTask(rightFwControlTask);
}
//short shooting
void initializePIDShort() {
//note the order of the parameters:
//(controller, motor ticks per rev, KpNorm, KpBallLaunch, Ki, Kd, constant, RPM drop on ball launch)
tbhInit(lFly, 392, .14491/* .12891.2958 0.3652*/, 1.7 /*2.77 3.037 1.0981 9.5*/, .005252, 0, 31.501, 15); //initialize PID for left side of the flywheel //left side might be able to have a higher P
tbhInit(rFly, 392, .14491/*.1291 0.3652*/, 1.7 /*3.037 1.943 9.5*/, .005252, 0, 31.501, 15); //initialize PID for right side of the flywheel //x.x481
startTask(leftFwControlTask);
startTask(rightFwControlTask);
}
//purple shooting (for skills)
void initializePIDMid() {
//note the order of the parameters:
//(controller, motor ticks per rev, KpNorm, KpBallLaunch, Ki, Kd, constant, RPM drop on ball launch)
tbhInit(lFly, 392, .1451, 1.70 /*3.24*/, 0.005052, 0, 39, 20); //initialize PID for left side of the flywheel //left side might be able to have a higher P
tbhInit(rFly, 392, .1451/*0.3791*/, 1.70, 0.005052, 0, 39, 20); //initialize PID for right side of the flywheel //x.x481
startTask(leftFwControlTask);
startTask(rightFwControlTask);
}
void progSkills()
{
FlywheelPower(60);
wait1Msec(500);
startTask(FlywheelController);
startTask(recoverFromShots);
SensorValue[ANGLE] = 0;
SetTarget(2260,44); // used to be 2050
driveAcross(300);
wait1Msec(150);
turnOther(100); // added by shlaok
//wait1Msec(500);
//turn(0);
while(first_cross)
{
wait1Msec(10);
}
startTask(AutonConveyor);/*
Conveyor(127);
//*/
//SensorValue[ANGLE] = 0;
//turnOther(120); // added by shlaok
while(shotCount<32)
{
wait1Msec(10);
}
turn(0);
driveStraight(-127);
wait1Msec(800);
motor[LeftDrive] = motor[RightDrive] = 0;
SensorValue[ANGLE] = 0;
/*stopTask(FlywheelController);
stopTask(recoverFromShots);
SetTarget(0,0);
FlywheelPower(0);*/
driveAcross(4350); // was 4600
//driveStraight(127);
//wait1Msec(300);
motor[LeftDrive] = motor[RightDrive] = 0;
turn(-120); // used to be positive
/*startTask(FlywheelController);
startTask(recoverFromShots);
FlywheelPower(60);
wait1Msec(300);
SetTarget(2000,34);
while(first_cross)
{
wait1Msec(10);
}*/
while(true)
{}
}
int bleutooth_control(string *s){
/*
This code is for taking over the robot completely using bleutooth to make sure you can stop de robot in case its needed.
pressing the middle button ("FIRE") on the phone stops the robot due to it not being included in the code here.
To get in to this function you have to press "B"
*/
TFileIOResult nBTCmdRdErrorStatus;
int nSizeOfMessage;
ubyte nRcvBuffer[kMaxSizeOfMessage];
int stopcode = 0;
while (*s != "A"){//if A is pressed the robot will resume its duty the normal way
nSizeOfMessage = cCmdMessageGetSize(INBOX);
if (nSizeOfMessage > kMaxSizeOfMessage){
nSizeOfMessage = kMaxSizeOfMessage;
}
if (nSizeOfMessage > 0){
nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
nRcvBuffer[nSizeOfMessage] = '\0';
*s = "";
stringFromChars(*s, (char *) nRcvBuffer);//put bluetooth input in string s
displayCenteredBigTextLine(4, *s);
}
//The next 4 if statements are for controlling the robot manually
if(*s == "LEFT"){//if bleutooth input is left turn left
motor(motorA) = 0;
motor(motorB) = 30;
startTask(music);//make sure the music is running
}
else if(*s == "RIGHT"){//if bleutooth input is right turn right
motor(motorA) = 30;
motor(motorB) = 0;
startTask(music);
}
else if (*s == "UP"){//if bleutooth input is up drive forward
setMultipleMotors(50, motorB, motorA);
startTask(music);
}
else if (*s == "DOWN"){//if bleutooth input is down drive backwards
setMultipleMotors(-50, motorB, motorA);
startTask(music);
}
else {//if there is no correct input turn off the motors
setMultipleMotors(0, motorA, motorB);
stopTask(music);//make sure the music is stopped
clearSounds();//empty the buffer
}
if (*s == "C"){//if the C button is pressed the loop stops and the stopcode == 1 to stop de code from running
stopcode = 1;
stopTask(music);
clearSounds();
break;
}
}
*s = "";//make sure s is empty
return stopcode;//output of the function used to stop the code if chosen
}
task main()
{
startTask(initialize);
startTask(displaySensorValues);
while(true)
{
wait1Msec(2);
}
}
void initializeTBHSkills() {
tbhInit(lFly, 627.2, 0.00825);//1025); //initialize TBH for left side of the flywheel
tbhInit(rFly, 627.2, 0.00825);//1000); //initialize TBH for the right side of the flywheel
//motor[intake] = 127;
//start the flywheel control tasks
startTask(leftFwControlTask);
startTask(rightFwControlTask);
startTask(flashLED);
}
//long shooting
void initializePIDLong() {
//note the order of the parameters:
//(controller, motor ticks per rev, KpNorm, KpBallLaunch, Ki, Kd, constant, RPM drop on ball launch)
tbhInit(lFly, 390.4, .1821/*0.4074*/, 3.64113/*2.69881 2.8*/, 0.005381, 0, 50, 20); //initialize PID for left side of the flywheel //left side might be able to have a higher P
tbhInit(rFly, 392, .1821/*0.4074*/, 3.64113, 0.005381, 0, 50, 20); //initialize PID for right side of the flywheel //x.x481
//tbhInit(lFly, 392, 0.3881, .6000, 0.006481, 0, 75, 20); //initialize PID for left side of the flywheel //left side might be able to have a higher P
//tbhInit(rFly, 392, 0.3881, .6000, 0.006481, 0, 75, 20); //initialize PID for right side of the flywheel //x.x481
startTask(leftFwControlTask);
startTask(rightFwControlTask);
}
void
task1 (void)
{
ticksPerSecond = rtems_clock_get_ticks_per_second();
createTask ('1', &taskId1);
createTask ('2', &taskId2);
startTask (taskId1, subTask1);
startTask (taskId2, subTask2);
rtems_task_suspend (RTEMS_SELF);
}
task main()
{
startTask(gyro_loop);
startTask(sonar_loop);
while(gyro_loop_state!=READING) sleep(5);
faceObject();
sleep(1000);
turn();
}
task main()
{
startTask(geluid);
while(1)
{
startTask(accelerate);
while (SensorValue(sonarsensor) > 5)
{
startTask(geluid);
while (SensorValue(sonarsensor) > 30)
{
if ((SensorValue(EyeLeft) == BLACKCOLOR) && (SensorValue(EyeRight) <= rightThreshold))
{
//
// Beide sensoren zien lijn, oftewel kruispunt.
//
clearSounds();
motor[left] = 0;
motor[right] = 0;
}
if ((SensorValue(EyeLeft) == BLACKCOLOR)&&(SensorValue(EyeRight)> rightThreshold))
{
//
// Only left sensor sees the line, we must turn left.
//
motor[left] = i;
motor[right] = i/(0.1 * speedlimit);
}
if ((SensorValue(EyeLeft) == WHITECOLOR) &&(SensorValue(EyeRight)< rightThreshold))
{
//
// Only right sensor sees the line, turn right.
//
motor[left] = i/(0.1 * speedlimit);
motor[right] = i;
}
if ((SensorValue(EyeLeft) == WHITECOLOR)&&(SensorValue(EyeRight) > rightThreshold))
{
motor[left] = i/2;
motor[right] = i/2;
}
wait1Msec(5);
}
clearSounds();
startTask(deaccelerate);
}
clearSounds();
i = 0;
}
}
task main()
{
clearDebugStream();
printnVexRCRecieveState();
startTask(MotorSlewRateTask);
startTask(DrivetrainControlTask);
startTask(LiftControlTask);
while (true) { EndTimeSlice(); }
}
task usercontrol()
{
startTask(Menu);
while (true)
{
startTask(driveComp);
startTask(armComp);
startTask(intakeComp);
}
}
task main()
{
waitForStart();
startTask(conveyerArm);
startTask(goalGrabber);
while(true)
{
joyDrive();
moveConveyer();
}
}
task main() {
const int autonomousDuration = 15 * 1000;
const int manualDuration = (60 + 45) * 1000;
startTask(hippo);
pause(autonomousDuration);
stopTask(hippo);
startTask(manual);
pause(manualDuration);
stopTask(manual);
}
