int main()
{
followLine();
set_servo_position(servo1port,upperServoPos);
rightTurnCP(500);
rightTurnCP(500);
followLine();
return 0;
}
//Rotation um 90° um eine Ecke auf der Streckenmarkierung, Übergabe der Richtung als deutsches Wort links/rechts in einem String
void rotate(unsigned char richtung){
//nach rechts??
if(richtung == 'rechts'){
setMotPow(motPowLeft,0);
setMotGear(forward,backward);
setMotPow(motPowLeft,10);
sleep(500);
do{
updateSensorsWhite();
}while(sensor[MID_RIGHT]);
setMotPow(motPowLeft,0);
setMotGear(forward,forward);
setMotPow(motPowLeft,motPowRight);
//nach links??
}else if (richtung == 'links'){
setMotPow(0,motPowRight);
setMotGear(backward,forward);
setMotPow(10,motPowRight);
sleep(500);
do{
updateSensorsWhite();
}while(sensor[MID_LEFT]);
setMotPow(0,motPowRight);
setMotGear(forward,forward);
setMotPow(motPowLeft,motPowRight);
}
followLine();
kurven++;
count = 0;
// lcd_cls();
// lcd_uint(count);
nachkurvetimeout = akt_time() + KURVSPERR;
}
uint8_t Move::forward(double target, int16_t speed, uint8_t allowedCrosses){
if(!drivingForward){
leftMotor->resetDistance();
rightMotor->resetDistance();
drivingForward = 1;
crossedLines = 0;
avgDistance = 0;
startDistance = 0;
acceptingCrosses = 1;
}
if(drivingForward && avgDistance<target)
{
if(followLine(speed) && acceptingCrosses && avgDistance > 3){
crossedLines++;
acceptingCrosses = 0;
startDistance = avgDistance;
}
if(avgDistance >= (startDistance + 3)) acceptingCrosses = 1;
avgDistance = leftMotor->getDistance()/2+rightMotor->getDistance()/2;
if(crossedLines > allowedCrosses){
drivingForward = 0;
return 1;
}
}
if(!drivingForward || avgDistance>=target) {
leftMotor->drive(0);
rightMotor->drive(0);
if(DEBUG)Serial.println("Forward move finished!");
drivingForward = 0;
return 1;
}
return 0;
}
int main(void) {
initIRSensor();
initMotor();
initGPIOLineSensor();
initServo();
SetServo(servo,0);
stage = 0;
//while(1){followLine();}
//while(1){followWall();}
//SetMotor(leftMotor, 1); SetMotor(rightMotor, 1);
while(true){
LineSensorReadArray(gls, line);
if (stage==0){ //start state
if(line[0]<0.5&&line[1]<0.5&&line[2]<0.5&&line[3]<0.5&&line[4]<0.5&&line[5]<0.5&&line[6]<0.5&&line[7]<0.5) {
followWall();
}else{
followLine();
}
}else if(stage==1){
//once 90degree turn passed
// SetMotor(leftMotor, 1);
// SetMotor(rightMotor, -1);
SetPin(PIN_F2, 1);
followWall();
if (wallPresent()) {followWall();}
else {
findLine();
}
SetPin(PIN_F2, 0);
}else if (stage==2){
//once line found again after walled section
SetPin(PIN_F1, 1);
if((line[0]<0.5&&line[1]<0.5&&line[2]<0.5&&line[3]<0.5&&line[4]<0.5&&line[5]<0.5&&line[6]<0.5&&line[7]<0.5)||
(mostDark())) { //line[0]>0.5&&line[1]>0.5&&line[2]>0.5&&line[3]>0.5&&line[4]>0.5&&line[5]>0.5&&line[6]>0.5&&line[7]>0.5)
findEnd();
}else{
followLine();
};
SetPin(PIN_F1, 0);
}else{//end of course look for flag
findObject();
break;
}
}
}
void fullDemo() {
int robotState = 0;
uint16_t sensorPattern[5] = {0};
while(robotState != 6) {
switch(robotState) {
case 0:
while(get_coord_x() < 200) {
forwards(20);
}
robotState = 1;
break;
case 1:
setSensorSide(1);
while(get_coord_x() < 400) {
correctForwardMotion();
}
robotState = 2;
break;
case 2:
while(convertToDeg(get_theta()) > -90) {
spinLeft();
}
while(get_coord_y() < 120) {
forwards(20);
}
while(convertToDeg(get_theta()) < 0) {
spinRight();
}
while(get_coord_x() < 600) {
forwards(20);
}
robotState = 3;
break;
case 3:
setSensorSide(2);
while(get_coord_x() < 800) {
correctForwardMotion();
}
robotState = 4;
break;
case 4:
forwards(20);
while(sensorPattern[3]<2000){
getRawSensors(sensorPattern);
}
robotState = 5;
break;
case 5:
followLine();
robotState = 6;
break;
}
}
}
int main(void) {
InitializeMCU();
// Drive at a slower (not turbo boosted) speed
// for a short period to prevent too fast of
// an acceleration at the start of the match.
setWheelMotors(SPEED, SPEED);
Wait(0.2);
// Follow the line and cross your fingers.
while (1) {
followLine();
}
}
void ProjectVisitor::ProjectTriangleToHeightField::insertTouchedLines(const osg::Vec3d& v1, const osg::Vec3d& v2)
{
osg::Vec2d p1InGrind, p2InGrind;
findGridFromPoint(v1, p1InGrind);
findGridFromPoint(v2, p2InGrind);
bool valid = clipLine(p1InGrind, p2InGrind);
if (!valid)
return;
osg::ref_ptr<osg::Vec3Array> array = followLine(p1InGrind, p2InGrind);
InsertGeometryHelper inserter;
inserter.init(_result.get());
inserter.appendLineStrip(array.get());
}
void AksenMain(void) {
starting();
do {
if (akt_time() >= timeoutAt) {
setMotPow(0,0);
break;
}
if(kurven == 8 && sensor[MID_MID] == 1 && sensor[MID_LEFT] == 1 && sensor[MID_RIGHT] == 1){
setMotPow(0,0);
break;
}
followLine();
countLines();
manage(start);
}while(1);
while(1);
}
/** Function to process all code-generator commands and return a
* quality ordered vector containing all generator output for
* the given route element
*/
vector<robotCommand> smrclGenerator::processCommands(vector<route> rPlan,size_t pIndex) {
const int nGenerators = 3;
vector<robotCommand> cmds;
robotCommand *newCmd = NULL;
//Return just the stop commands if pIndex is out of scope
if (pIndex >= rPlan.size()) {
newCmd = stopCmd(rPlan,pIndex);
cmds.push_back(*newCmd);
delete newCmd;
} else {
//Run all code generator functions
for (int i = 0; i < nGenerators; i++) {
//Fix so far, switch beween generators
switch (i) {
case 0:
newCmd = followLine(rPlan,pIndex);
break;
case 1:
newCmd = shortOdoDrive(rPlan,pIndex);
break;
case 2:
newCmd = fillDrive(rPlan,pIndex);
break;
};
//Only use commands, if quality is better than zero..
if (newCmd->quality > 0) cmds.push_back(*newCmd);
delete newCmd;
}
}
//Sort the commands by quality using the STL sorting algorithm
//and reverse it to have largest quality first
sort(cmds.begin(),cmds.end());
reverse(cmds.begin(),cmds.end());
return cmds;
}
void rotateFrei(unsigned char richtung){
//nach rechts??
if(richtung == 'rechts'){
messung_start = akt_time() + messung-25;
setMotPow(motPowLeft,0);
setMotGear(forward,backward);
setMotPow(motPowLeft,10);
sleep(500);
do{
}while(akt_time()<=messung_start);
setMotPow(motPowLeft,0);
setMotGear(forward,forward);
setMotPow(motPowLeft,motPowRight);
//nach links??
}else if (richtung == 'links'){
messung_start = akt_time() + messung + 60;
setMotPow(0,motPowRight);
setMotGear(backward,forward);
setMotPow(10,motPowRight);
sleep(500);
do{
}while(akt_time()<=messung_start);
setMotPow(0,motPowRight);
setMotGear(forward,forward);
//motPowLeft=motPowLeft-1;
setMotPow(motPowLeft,motPowRight);
}
followLine();
kurven++;
count = 0;
//lcd_cls();
//lcd_uint(count);
nachkurvetimeout = akt_time() + KURVSPERR;
}
int main(void)
{
SYSTEMConfigPerformance(40000000);
initLCD(); //initialize the LCD
enableInterrupts();
initADC(); //initialize the ADC
initPWM(); //initialize the PWM
initTimer3(); //initialize the timer
TRISDbits.TRISD0 = 0;
LATDbits.LATD0 = 0;
initUART(); //initialize UART
sendByte('M');
while(1)
{
if(U2STAbits.URXDA)
{
input = U2RXREG; //saves RX reg buffer
sendByte(input); //echos pressed key back to prompt
sendByte(':'); //sends : to terminal prompt
}
switch(input) {
case 'w': //forward
LEFTMOTORDIRECTION1 = 0;
LEFTMOTORDIRECTION2 = 1;
RIGHTMOTORDIRECTION1 = 0;
RIGHTMOTORDIRECTION2 = 1;
RW = 700;
LW = 700;
break;
case 'a': //left
LEFTMOTORDIRECTION1 = 0;
LEFTMOTORDIRECTION2 = 1;
RIGHTMOTORDIRECTION1 = 0;
RIGHTMOTORDIRECTION2 = 1;
RW = 700;
LW = 100;
break;
case 's': //backward
LEFTMOTORDIRECTION1 = 1;
LEFTMOTORDIRECTION2 = 0;
RIGHTMOTORDIRECTION1 = 1;
RIGHTMOTORDIRECTION2 = 0;
RW = 700;
LW = 700;
break;
case 'd': //right
LEFTMOTORDIRECTION1 = 0;
LEFTMOTORDIRECTION2 = 1;
RIGHTMOTORDIRECTION1 = 0;
RIGHTMOTORDIRECTION2 = 1;
RW = 100;
LW = 700;
break;
case 'p':
LEFTMOTORDIRECTION1 = 0;
LEFTMOTORDIRECTION2 = 1;
RIGHTMOTORDIRECTION1 = 0;
RIGHTMOTORDIRECTION2 = 1;
RW = 0;
LW = 0;
break;
}
startRead(0); //starts reading from ADC (POT)
adcVal0 = waitToFinish0(); //save digital value of pot
startRead(1); //starts reading from ADC (Leftmost)
adcVal1 = waitToFinish1(); //save digital value of leftmost phototransistor
startRead(2); //starts reading from ADC (Middle)
adcVal2 = waitToFinish2(); //save digital value of middle phototransistor
startRead(3); //starts reading from ADC (Rightmost)
adcVal3 = waitToFinish3(); //save digital value of rightmost phototransistor
moveCursorLCD(0,2);
sprintf(str, "%d,%d,%d", adcVal1/10, adcVal2/10, adcVal3/10);
printStringLCD(str);
scan(); //scan three transistors
moveCursorLCD(0,1);
sprintf(str, "%d, %d, %d", L1, L2, L3);
printStringLCD(str);
determineState();
switch(jason)
{
case followLine_detectEnd:
followLine();
detectEnd();
break;
case turnAroundJason:
turnAround();
break;
}
}
return 0;
}
void Autonomous(void)
{
#if 1
/*int autoMode = 0;
autoMode |= bcd1->Get();
autoMode <<= 1;
autoMode |= bcd2->Get();
autoMode <<= 1;
autoMode |= bcd3->Get()
;*/
//double ignoreLineStart = 0;
GetWatchdog().SetEnabled(true);
GetWatchdog().SetExpiration(0.2);
float liftSP = 0;
PIDLift->SetTolerance(10);
PIDLift->SetContinuous(false);
PIDLift->SetOutputRange(-0.75, 1); //BUGBUG
PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN);
PIDLift->Enable();
PIDGrip->SetSetpoint(0);
PIDGrip->Enable();
stopCount = 0;
float reduction;
int counts = 0;
leftEncoder->Start();
rightEncoder->Start();
leftEncoder->Reset();
rightEncoder->Reset();
liftEncoder->Start();
liftEncoder->Reset();
leftEncoder->SetDistancePerPulse((6 * PI / 500)/reduction);
rightEncoder->SetDistancePerPulse((6 * PI / 500)/reduction);
double avgEncoderCount;
float leftSpeed = .2, rightSpeed = .2;
short int lsL,lsM,lsR;
lineFollowDone = false;
counts = 0;
//int fancyWaiter = 0;
int popstage = 0;
goPop = false;
double backStart = 0;
double backTime = 0;
double popStart = 0;
double popTime = 0;
bool backDone = false;
miniDep->Set(miniDep->kForward);
int liftCount = 0;
bool disengageBrake = false;
float lastLiftSP = 0;
gripOpen1->Set(true);
gripOpen2->Set(false);
gripLatch1->Set(true);
gripLatch2->Set(false);
while(IsAutonomous())
{
if(!(counts % 100))printf("%2.2f \n",getDistance());
if(backStart) backTime = GetClock();
if(popStart) popTime = GetClock();
//if(!ignoreLineStart)ignoreLineStart = GetClock();
if(!compSwitch->Get()) compressor->Set(compressor->kReverse);
else compressor->Set(compressor->kOff);
if(counts%3 == 0)
{
leftValue = lsLeft->Get();
middleValue = lsMiddle->Get();
rightValue = lsRight->Get();
}
counts++;
GetWatchdog().Feed();
//avgEncoderCount = (leftEncoder->Get() + rightEncoder->Get())/2;
//myRobot.SetLeftRightMotorOutputs(.2,.2);
//All three/five autonomous programs will do the same thing up until 87 inches from the wall
if (counts % 100 == 0){//TESTING
if(lsLeft->Get()){
lsL = 1;
}else{
lsL = 0;
}
if(lsRight->Get()){
lsR = 1;
}else{
lsR = 0;
}
if(lsMiddle->Get()){
lsM = 1;
}else{
lsM = 0;
}
//printf("Encoder: %2.2f \n", (float)avgEncoderCount);
//printf("Distance: %2.2f SensorL:%1d SensorM:%1d SensorR:%1d\n",getDistance(),lsL,lsM,lsR);//TESTING
}
#if FOLLOWLINE
/*if(GetClock() - ignoreLineStart <= 0.5)
{
frontLeftMotor->Set(-.4);
rearLeftMotor->Set(-.4);
frontRightMotor->Set(.4);
rearRightMotor->Set(.4);
}
else */if (false){//(avgEncoderCount <= SECONDBREAKDISTANCE){
followLine();
}
#else
if (getDistance() > 24){
frontLeftMotor->Set(-leftSpeed);
rearLeftMotor->Set(-leftSpeed);
frontRightMotor->Set(rightSpeed);
rearRightMotor->Set(rightSpeed);
if(leftEncoder->Get() > rightEncoder->Get() && leftSpeed == .2){
rightSpeed += .03;
}else if(leftEncoder->Get() >rightEncoder->Get() && leftSpeed > .2){
leftSpeed -= .03;
}else if(leftEncoder->Get() < rightEncoder->Get() && rightSpeed == .2){
leftSpeed += .03;
}else if(leftEncoder->Get() < rightEncoder->Get() && rightSpeed > .2){
rightSpeed -= .03;
}
}
#endif
else{
if(counts % 100 == 0) {printf("DISTANCE: %2.2f\n",getDistance());}
switch(FOLLOWLINE)
{
case STRAIGHTLINEMODE: //Straight line. Scores on middle column of left or right grid.
//if(lineFollowDone && !(counts %50)) printf("Lift error: %f \n", PIDLift->GetError());
lastLiftSP = liftSP;
if(!lineFollowDone)
{
followLine();
}
else if(!PIDLift->GetSetpoint() && !popstage && !backStart)
{
//if(counts % 50 == 0)printf("Go backward\n");
frontLeftMotor->Set(.3);
rearLeftMotor->Set(.3);
frontRightMotor->Set(-.3);
rearRightMotor->Set(-.3);
//PIDLift->SetSetpoint(LIFTMID2);
liftSP = LIFTHIGH2 + 0.025;
//fancyWaiter = counts;
backStart = GetClock();
printf("Setpoint set setpoint set setpoint set \n");
/*
if(leftValue && middleValue && rightValue)
{
printf("Stopped 2nd time\n");
goPop = true;
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
PIDLift->SetSetpoint(LIFTHIGH2);
}
*/
}
#if 1 //if we've backed up for half a second and we're not popping
else if((backTime - backStart) > 0.65 && !backDone)
{
printf("Stopping!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!ONE\n");
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
//PIDLift->SetSetpoint(LIFTMID2);
liftSP = LIFTHIGH2;
backDone = true;
//Wait(.01);
//lift->brakeOff();
//fancyWaiter = counts;
//printf("Fancy waiter set Fancy waiter set Fancy waiter set");
}
/*
else if(lift->getPosition() < LIFTHIGH2)
{
//Move teh lifts
//if(counts % 100 == 0) printf("Stopping because lineFollowDone == true\n");
PIDLift->SetSetpoint(LIFTHIGH2);
}
*/
//if the lift is at the top and we're done backing up
else if(PIDLift->GetSetpoint() && fabs(liftSP - lift->getPosition()) < 0.015 && backDone)
{
if(!popStart) popStart = GetClock();
if(popstage == 0)
{
//lift->brakeOn();
//PIDLift->SetSetpoint(lift->getPosition());
popstage++;
printf("BRAKE BRAKE BRAKE BRAKE BRAKE \n");
}
else if(popstage == 1 && popTime - popStart > 0.1)
{
#if !(INNERGRIP)
gripOpen1->Set(true);
gripOpen2->Set(false);
#else
gripOpen1->Set(false);
gripOpen2->Set(true);
#endif
popstage++;
printf("OPEN OPEN OPEN OPEN OPEN \n");
}
else if(popstage == 2 && popTime - popStart > 0.35)
{
gripPop1->Set(true);
gripPop2->Set(false);
popstage++;
printf("POP POP POP POP POP POP POP \n");
}
else if(popstage == 3 && popTime - popStart > 1.35)
{
gripPop1->Set(false);
gripPop2->Set(true);
frontLeftMotor->Set(.2);
rearLeftMotor->Set(.2);
frontRightMotor->Set(-.2);
rearRightMotor->Set(-.2);
popstage++;
printf("UNPOP UNPOP UNPOP UNPOP UNPOP \n");
}
else if(popstage == 4 && popTime - popStart > 1.85)
{
printf("DOWN DOWN DOWN DOWN DOWN DOWN \n");
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
//PIDLift->SetSetpoint(0);
liftSP = 0;
}
/*
else if(popstage == 4 && popTime - popStart > 1.85)
{
printf("DOWN DOWN DOWN DOWN DOWN DOWN \n");
frontLeftMotor->Set(min(0.3, 0.25*(popTime - popStart - 1.85)));
rearLeftMotor->Set(min(0.3, 0.25*(popTime - popStart - 1.85)));
frontRightMotor->Set(max(-0.3, -0.25*(popTime - popStart - 1.85)));
rearRightMotor->Set(max(-0.3, -0.25*(popTime - popStart - 1.85)));
//PIDLift->SetSetpoint(0);
liftSP = 0;
popstage++;
}
else if(popstage == 5 && popTime - popStart > 4.85)
{
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
}
*/
}
//Start tele-op lift code
if(!lift->isBraking() && !disengageBrake)
{
PIDLift->SetSetpoint(liftSP);
if(liftSP == 0 && liftPIDSource->PIDGet() < 0.1)
{
//PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, 3*LIFTDERIVGAIN);
PIDLift->SetOutputRange(-liftPIDSource->PIDGet() - 0.1, 1);
}
else PIDLift->SetOutputRange(-0.75, 1);
}
if(lift->isBraking() && lastLiftSP != liftSP)
{
PIDLift->SetSetpoint(lift->getPosition() + 0.04);
PIDLift->SetPID(11.5 + 2*lift->getPosition(), LIFTINTGAIN + 0.4 + 3*lift->getPosition()/10, 0);
lift->brakeOff();
disengageBrake = true;
if(!liftCount) liftCount = counts;
}
//set brake (because near)
if(fabs(PIDLift->GetError()) < 0.015 && !lift->isBraking() && !disengageBrake)
{
if(liftCount == 0) liftCount = counts;
if(counts - liftCount > 1000)
{
PIDLift->Reset();
liftMotor1->Set(LIFTNEUTRALPOWER);
liftMotor2->Set(LIFTNEUTRALPOWER);
Wait(0.02);
lift->brakeOn();
Wait(0.02);
liftMotor1->Set(0.0);
liftMotor2->Set(0.0);
PIDLift->Enable();
//PIDLift->SetSetpoint(lift->getPosition());
liftCount = 0;
}
if(counts%3000) printf("Braking/Not PID \n");
}
if(lift->isBraking() && !(counts%100000)) PIDLift->SetSetpoint(lift->getPosition());
if(fabs(PIDLift->GetError()) < 0.01 && disengageBrake && counts - liftCount > 1000)
{
disengageBrake = false;
PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN);
liftCount = 0;
}
//End tele-op lift code
#endif
//myRobot.SetLeftRightMotorOutputs(0,0);
break;
case NOLINEMODE: //Fork turn left. Scores on far right column of left grid.
lineFollowDone = true;
if(!lineFollowDone){}
else if(!PIDLift->GetSetpoint() && !popstage && !backStart)
{
//if(counts % 50 == 0)printf("Go backward\n");
frontLeftMotor->Set(.3);
rearLeftMotor->Set(.3);
frontRightMotor->Set(-.3);
rearRightMotor->Set(-.3);
//PIDLift->SetSetpoint(LIFTMID2);
liftSP = LIFTHIGH2;
//fancyWaiter = counts;
backStart = GetClock();
printf("Setpoint set setpoint set setpoint set \n");
/*
if(leftValue && middleValue && rightValue)
{
printf("Stopped 2nd time\n");
goPop = true;
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
PIDLift->SetSetpoint(LIFTHIGH2);
}
*/
}
#if 1 //if we've backed up for half a second and we're not popping
else if((backTime - backStart) > 0.65 && !backDone)
{
printf("Stopping!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!ONE\n");
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
//PIDLift->SetSetpoint(LIFTMID2);
liftSP = LIFTHIGH2;
backDone = true;
//Wait(.01);
//lift->brakeOff();
//fancyWaiter = counts;
//printf("Fancy waiter set Fancy waiter set Fancy waiter set");
}
/*
else if(lift->getPosition() < LIFTHIGH2)
{
//Move teh lifts
//if(counts % 100 == 0) printf("Stopping because lineFollowDone == true\n");
PIDLift->SetSetpoint(LIFTHIGH2);
}
*/
//if the lift is at the top and we're done backing up
else if(PIDLift->GetSetpoint() && fabs(liftSP - lift->getPosition()) < 0.015 && backDone)
{
if(!popStart) popStart = GetClock();
if(popstage == 0)
{
//lift->brakeOn();
//PIDLift->SetSetpoint(lift->getPosition());
popstage++;
printf("BRAKE BRAKE BRAKE BRAKE BRAKE \n");
}
else if(popstage == 1 && popTime - popStart > 0.1)
{
#if !(INNERGRIP)
gripOpen1->Set(true);
gripOpen2->Set(false);
#else
gripOpen1->Set(false);
gripOpen2->Set(true);
#endif
popstage++;
printf("OPEN OPEN OPEN OPEN OPEN \n");
}
else if(popstage == 2 && popTime - popStart > 0.35)
{
gripPop1->Set(true);
gripPop2->Set(false);
popstage++;
printf("POP POP POP POP POP POP POP \n");
}
else if(popstage == 3 && popTime - popStart > 1.35)
{
gripPop1->Set(false);
gripPop2->Set(true);
frontLeftMotor->Set(.2);
rearLeftMotor->Set(.2);
frontRightMotor->Set(-.2);
rearRightMotor->Set(-.2);
popstage++;
printf("UNPOP UNPOP UNPOP UNPOP UNPOP \n");
}
else if(popstage == 4 && popTime - popStart > 1.85)
{
printf("DOWN DOWN DOWN DOWN DOWN DOWN \n");
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
//PIDLift->SetSetpoint(0);
liftSP = 0;
}
}
//Start tele-op lift code
if(!lift->isBraking() && !disengageBrake)
{
PIDLift->SetSetpoint(liftSP);
if(liftSP == 0 && liftPIDSource->PIDGet() < 0.1) //BUGBUG
{
//PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, 3*LIFTDERIVGAIN);
PIDLift->SetOutputRange(-liftPIDSource->PIDGet() - 0.1, 1.); //BUGBUG
}
else PIDLift->SetOutputRange(-0.75, 1.); //BUGBUG
}
if(lift->isBraking() && lastLiftSP != liftSP)
{
PIDLift->SetSetpoint(lift->getPosition() + 0.04);
PIDLift->SetPID(11.5 + 2*lift->getPosition(), LIFTINTGAIN + 0.4 + 3*lift->getPosition()/10, 0);
lift->brakeOff();
disengageBrake = true;
if(!liftCount) liftCount = counts;
}
//set brake (because near)
if(fabs(PIDLift->GetError()) < 0.015 && !lift->isBraking() && !disengageBrake)
{
if(liftCount == 0) liftCount = counts;
if(counts - liftCount > 1000)
{
PIDLift->Reset();
liftMotor1->Set(LIFTNEUTRALPOWER);
liftMotor2->Set(LIFTNEUTRALPOWER);
Wait(0.02);
lift->brakeOn();
Wait(0.02);
liftMotor1->Set(0.0);
liftMotor2->Set(0.0);
PIDLift->Enable();
//PIDLift->SetSetpoint(lift->getPosition());
liftCount = 0;
}
if(counts%3000) printf("Braking/Not PID \n");
}
if(lift->isBraking() && !(counts%100000)) PIDLift->SetSetpoint(lift->getPosition());
if(fabs(PIDLift->GetError()) < 0.01 && disengageBrake && counts - liftCount > 1000)
{
disengageBrake = false;
PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN);
liftCount = 0;
}
//End tele-op lift code
#endif
//myRobot.SetLeftRightMotorOutputs(0,0);
break;
case FORKRIGHTMODE://Fork turn right. Scores on far left column of right grid.
if(leftEncoder->GetDistance() <= rightEncoder->GetDistance() + 6)
{
frontLeftMotor->Set(.2);
rearLeftMotor->Set(.2);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
}
else if(getDistance() >= SCOREDISTANCE)
{
followLine();
}
//score here
//myRobot.SetLeftRightMotorOutputs(0,0);
break;
}
}
}
/*frontRightMotor->Set(0);
rearRightMotor->Set(0);
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);*/
Wait(.02);
#endif
}
void doTheDemo() {
cmdDoPlay(">cc");
_DBG_("State 0");
forwards(20);
cmdDoPlay(">dd");
_DBG_("State 1");
while (sensorSide != 1) {
//Forwards until we find a left wall
checkForWall();
_DBD(sensorSide);_DBG_(" sens");
}
cmdDoPlay(">ee");
_DBG_("State 2");
//Follow wall until it's ended
int wallState = -1;
while (wallState != 3 && wallState != 0) {
wallState = checkForWall();
correctForwardMotion();
}
if (courseType == 0) {
cmdDoPlay(">aa");
_DBG_("State 5");
while(!checkForLine()) {
}
followLine();
while(!checkForNoLine()) {
}
brake();
}
else {
//Bear right to head for other wall
cmdDoPlay(">ff");
_DBG_("State 3");
right();
delay(1000);
_DBG_("State 3.1");
forwards(20);
//Wait until right wall is trackable
while (sensorSide != 2) {
checkForWall();
_DBD(sensorSide);_DBG_(" sens");
}
//Track right wall
cmdDoPlay(">gg");
_DBG_("State 4");
wallState = -1;
while (wallState != 4 && wallState != 0) {
wallState = checkForWall();
correctForwardMotion();
}
//Find the line
cmdDoPlay(">aa");
_DBG_("State 5");
forwards(20);
while(!checkForLine()) {
}
followLine();
while(!checkForNoLine()) {
}
brake();
}
}
bool NavSystem::NavActivity::run() {
unsigned long now = runningTime();
unsigned long timeSinceLastState = now-lastStateTime;
unsigned long timeSinceLastLine = now-lastLineTime;
// Always read line sensor (so we know where the line was last seen
unsigned int sensorValues[NUM_SENSORS];
long position = nav->qtrrc8.readLine(sensorValues);
// Check if we've crossed an intersection
// This is checked by seening if the average of the sensor values is >900
if (timeSinceLastLine > kLineTimeout) {
long sum = 0;
for (int i = 0; i < NUM_SENSORS; ++i) {
sum += sensorValues[i];
}
if (sum > kIntersectionThresh*NUM_SENSORS) {
// If we've found and intersection, and are currently counting intersections,
// count it
if (nav->current_command.type == FOLLOW_COUNT) --nav->current_command.data;
// Reset time since we last saw a line.
lastLineTime = now;
}
}
// If the robot is not enabled, stop driving, and return
if(!nav->robot->status.enabled()) {
nav->stop();
return false;
}
switch (nav->current_command.type) {
case BACK_UP: {
int back_up_time = kNormalBackupTime;
if (nav->current_pos == REACTOR_B ||
nav->current_pos == REACTOR_A ) {
back_up_time = kReactorBackupTime;
}
if (timeSinceLastState > back_up_time) {
nav->stop();
nav->next();
lastStateTime = now;
} else {
nav->drive(50,-50);
}
break;
}
case FORWARD:
if (timeSinceLastState > 200) {
nav->stop();
nav->next();
lastStateTime = now;
} else {
nav->drive(-50,50);
}
break;
// Turning around follows the same process as turning left or right:
// turn until you don't see the line, then turn until the line is
// roughly centered.
case TURN_AROUND:
case TURN_LEFT: {
long sum = 0;
for (int i = 0; i < NavSystem::NUM_SENSORS; ++i) {
sum += sensorValues[i];
}
if (sum < 100*NUM_SENSORS) { nav->current_command.data = 1; }
if ((nav->current_command.data == 1) && ((position > 3000 && position < 4000))) {
nav->stop();
nav->next();
lastStateTime = now;
} else {
nav->drive(60,60);
}
break;
}
case TURN_RIGHT: {
long sum = 0;
for (int i = 0; i < NavSystem::NUM_SENSORS; ++i) {
sum += sensorValues[i];
}
if (sum < kNoLineThresh*NUM_SENSORS) { nav->current_command.data = 1; }
if ((nav->current_command.data == 1) && ((position > 3000 && position < 4000))) {
nav->stop();
nav->next();
lastStateTime = now;
} else {
nav->drive(-60,-60);
}
break;
}
case FOLLOW_LIMIT:
if (digitalRead(FRONT_BUMP_PORT)) {
followLine(position);
} else {
nav->current_pos = nav->desired_pos;
nav->current_dir = nav->desired_dir;
lastStateTime = now;
nav->next();
nav->stop();
}
break;
case FOLLOW_COUNT:
followLine(position);
if (nav->current_command.data <= 0) {
nav->stop();
nav->next();
lastStateTime = now;
}
break;
case DONE:
nav->stop();
break;
}
return false;
}
void lineFollower() {
setMorpheus();
strafeUntilLine();
followLine();
}
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 startLineFollowing(int spd) {
oldError = 0;
setMotion(spd, 0);
followLine(spd);
TMRArd_InitTimer(LINE_FOLLOW_TIMER, LINE_FOLLOW_UPDATE_PERIOD);
}
/*
* Slot wrapper for MobilePlatform::followLine()
* @brief MobilePlatform::slot_followLine()
*/
void MobilePlatform :: slot_followLine()
{
followLine();
}
