void OperatorControl(void) { AxisCamera &camera = AxisCamera::GetInstance(); miniBotTime.Start(); initRobot(); debug("in telop"); compressor.Start(); GetWatchdog().SetEnabled(true); /*int l1, l2, l3; while (IsOperatorControl()) { GetWatchdog().Feed(); //char val = (line1.Get() & 0x01) | (line2.Get() & 0x02) | (line3.Get() & 0x04); //if(l1 != line1.Get() || l2 != line2.Get() || l3 != line3.Get()) { // cerr << "change " << (l1 = line1.Get()) << "\t" << (l2 = line2.Get()) << "\t" << (l3 = line3.Get()) << endl; //} cerr << "Change "<< line1.Get() <<"\t" << line2.Get() << "\t" << line3.Get() << endl; Wait(0.2); }*/ char count=0, pneumaticCount=0; // was .125 when loop at .025 lowPass lowSpeed(.04), lowStrafe(.04), lowTurn(.04), lowClaw(.04), lowArm(.04), lowArmLoc(.05); double ClawLocation=0, ArmLocation=0, OldArmLocation=0; while (IsOperatorControl() && !IsDisabled()) { GetWatchdog().Feed(); float speed = -1*stick.GetRawAxis(2); float strafe = stick.GetRawAxis(1); float turn = stick.GetRawAxis(3); if(!stick.GetRawButton(7)) { speed /= 2; strafe /= 2; turn /= 2; } if(stick.GetRawButton(8)) { speed /= 2; strafe /= 2; turn /= 2; } if(stick.GetRawButton(2)) { speed = 0; turn = 0; } Drive(lowSpeed(speed), lowTurn(turn), lowStrafe(strafe)); #ifndef NDEBUG if(stick2.GetRawButton(10)) { robotInted = false; initRobot(); } #endif if(stick2.GetRawButton(7) && (miniBotTime.Get() >= 110 || (stick2.GetRawButton(9) && stick2.GetRawButton(10)))) { // launcher // the quick launcher MiniBot1a.Set(true); MiniBot1b.Set(false); } if(!stick2.GetRawButton(10) && stick2.GetRawButton(9)) { // deploy in MiniBot2a.Set(false); MiniBot2b.Set(true); //MiniBot2a.Set(false); //MiniBot2b.Set(true); } if(stick2.GetRawButton(5)) { // top deploy out MiniBot2a.Set(true); MiniBot2b.Set(false); } if(stick2.GetRawButton(6)) { // open ClawOpen.Set(true); ClawClose.Set(false); } if(stick2.GetRawButton(8)) { // closed ClawOpen.Set(false); ClawClose.Set(true); ClawLocation += 2; } /*156 straight * 56 90 angle * 10 back */ if(stick2.GetRawButton(1)) { // top peg ClawLocation = 156; ArmLocation = 105; } if(stick2.GetRawButton(2)) { ClawLocation = 111; // the 90angle / middle peg ArmLocation = 50; } if(stick2.GetRawButton(3)) { // off ground ClawLocation = 176; ArmLocation = 5; } if(stick2.GetRawButton(4)) { ClawLocation = 0; // back ArmLocation = 0; } double tmpClaw = .7*lowClaw(stick2.GetRawAxis(4)); if(tmpClaw < .2 && tmpClaw > -.2) tmpClaw = 0; double tmpArm = .4*lowArm(-1*stick2.GetRawAxis(2)); if(tmpArm < .2 && tmpArm > -.2) tmpArm = 0; if(tmpArm > .5) tmpArm = .5; if(tmpArm < -.5) tmpArm = -.5; ClawLocation += tmpClaw + tmpArm; // the right joy stick y if(ClawLocation < 10) ClawLocation = 10; if(ClawLocation > 230) ClawLocation = 230; Claw(ClawLocation); ArmLocation += tmpArm; if(ArmLocation > 110) ArmLocation = 110; if(ArmLocation < -10) ArmLocation = -10; Arm(lowArmLoc(ArmLocation)); OldArmLocation = ArmLocation; #ifndef NDEBUG if(count++%20==0){ cerr << EncClaw.Get() << '\t' << arm1->GetOutputCurrent() << '\t' << arm1_sec->GetOutputCurrent() << '\t' << ArmLocation << '\t' << EncArm.Get() << endl; // cerr << arm1->GetOutputCurrent() << '\t' << arm1_sec->GetOutputCurrent() << '\t' << arm2->GetOutputCurrent() << '\t' // << EncArm.Get () << '\t' << LimitArm.Get() << '\t' << EncClaw.Get() << '\t' << LimitClaw.Get() << '\t' << ClawLocation << endl; //cerr << '\t' << EncArm.Get() <<'\t' << arm1->Get() << endl; // cerr << Dlf->Get() << '\t' << Dlf->GetSpeed() << '\t' << Dlb->GetSpeed() <<'\t' << Drf->GetSpeed() <<'\t' << Drb->GetSpeed() <<endl;//'\t' << line1.Get() << "\t" << line2.Get() << "\t" << line3.Get() << endl; // cerr << '\t' << Dlb->GetSpeed() << '\t'; } #endif if(pneumaticCount++==0) { ClawOpen.Set(false); ClawClose.Set(false); MiniBot1a.Set(false); MiniBot1b.Set(false); MiniBot2a.Set(false); MiniBot2b.Set(false); } Wait(0.01); // wait for a motor update time } }
void RunWheels() { // uint32_t t0, t1, t2, t3; // schedule updates to avoid overloading CAN bus or CPU switch (report++) { case 12: // 240 milliseconds report = 0; // reset counter case 0: // Update PID parameters double newP = SmartDashboard::GetNumber("Shooter P"); double newI = SmartDashboard::GetNumber("Shooter I"); double newD = SmartDashboard::GetNumber("Shooter D"); if (newP != kP || newI != kI || newD != kD) { kP = newP; kI = newI; kD = newD; #ifdef HAVE_TOP_WHEEL if (topPID) { #ifdef HAVE_TOP_CAN1 ; // topWheel1->SetPID( kP, kI, kD ); #endif #ifdef HAVE_TOP_CAN2 topWheel2->SetPID( kP, kI, kD ); #endif } #endif #ifdef HAVE_BOTTOM_WHEEL if (bottomPID) { #ifdef HAVE_BOTTOM_CAN1 ; // bottomWheel1->SetPID( kP, kI, kD ); #endif #ifdef HAVE_BOTTOM_CAN2 bottomWheel2->SetPID( kP, kI, kD ); #endif } #endif } break; case 4: // 80 milliseconds #ifdef HAVE_TOP_WHEEL //t0 = GetFPGATime(); // Get top output voltage, current and measured speed #ifdef HAVE_TOP_CAN1 double topI1 = topWheel1->GetOutputCurrent(); #endif #ifdef HAVE_TOP_CAN2 double topI2 = topWheel2->GetOutputCurrent(); topJagSpeed = topWheel2->GetSpeed(); #endif //t1 = GetFPGATime(); topTachSpeed = topTach->PIDGet(); #ifdef HAVE_TOP_CAN1 // stupid floating point! Log(LOG_CURRENT, 1, (uint32_t)(topI1 * 1000 + 0.5)); #endif #ifdef HAVE_TOP_CAN2 Log(LOG_CURRENT, 2, (uint32_t)(topI2 * 1000 + 0.5)); Log(LOG_SPEED, 2, (uint32_t)(topJagSpeed + 0.5)); #endif // Send values to SmartDashboard #ifdef HAVE_TOP_CAN1 SmartDashboard::PutNumber("Top Current 1", topI1); #endif #ifdef HAVE_TOP_CAN2 SmartDashboard::PutNumber("Top Current 2", topI2); SmartDashboard::PutNumber("Top Jag ", topJagSpeed); #endif SmartDashboard::PutNumber("Top Tach ", topTachSpeed); // Get setpoint topSpeed = SmartDashboard::GetNumber("Top Set "); //t2 = GetFPGATime(); if (spinFastNow) { if (topPID) { if (topJagSpeed < topSpeed * vbusThreshold) { topPID = false; // below threshold: switch both motors to full output #ifdef HAVE_TOP_CAN1 jagVbus(topWheel1, maxOutput); Log(LOG_MODE, 1, 1); #endif #ifdef HAVE_TOP_PWM1 topWheel1->Set(maxOutput); Log(LOG_MODE, 1, 1); #endif #ifdef HAVE_TOP_CAN2 jagVbus(topWheel2, maxOutput); Log(LOG_MODE, 2, 1); #endif } else { ; // above threshold: run motor 1 off, PID on motor 2 #ifdef HAVE_TOP_CAN1 topWheel1->Set(0.0); #endif #ifdef HAVE_TOP_PWM1 topWheel1->Set(0.0); #endif #ifdef HAVE_TOP_CAN2 topWheel2->Set(topSpeed); #endif } } else { if (topJagSpeed >= topSpeed * pidThreshold) { ; // above threshold: switch motor 1 off, motor 2 PID topPID = true; #ifdef HAVE_TOP_CAN1 topWheel1->Set(0.0); #endif #ifdef HAVE_TOP_PWM1 topWheel1->Set(0.0); #endif #ifdef HAVE_TOP_CAN2 jagPID(topWheel2, topSpeed); Log(LOG_MODE, 2, 2); #endif } else { ; // below threshold: run both motors at full output #ifdef HAVE_TOP_CAN1 topWheel1->Set(maxOutput); #endif #ifdef HAVE_TOP_PWM1 topWheel1->Set(maxOutput); #endif #ifdef HAVE_TOP_CAN2 topWheel2->Set(maxOutput); #endif } } //t3 = GetFPGATime(); //printf("%10u %10u %10u\n", (uint32_t)(t1 - t0), (uint32_t)(t2 - t1), (uint32_t)(t3 - t2)); } #endif break; case 8: // 160 milliseconds #ifdef HAVE_BOTTOM_WHEEL // Get bottom output voltage, current and measured speed //t0 = GetFPGATime(); #ifdef HAVE_BOTTOM_CAN1 double bottomI1 = bottomWheel1->GetOutputCurrent(); #endif #ifdef HAVE_BOTTOM_CAN2 double bottomI2 = bottomWheel2->GetOutputCurrent(); bottomJagSpeed = bottomWheel2->GetSpeed(); #endif //t1 = GetFPGATime(); bottomTachSpeed = bottomTach->PIDGet(); #ifdef HAVE_BOTTOM_CAN1 Log(LOG_CURRENT, 3, (uint32_t)(bottomI1 * 1000 + 0.5)); #endif #ifdef HAVE_BOTTOM_CAN2 Log(LOG_CURRENT, 4, (uint32_t)(bottomI2 * 1000 + 0.5)); Log(LOG_SPEED, 4, (uint32_t)(bottomJagSpeed + 0.5)); #endif // Send values to SmartDashboard #ifdef HAVE_BOTTOM_CAN1 SmartDashboard::PutNumber("Bottom Current 1", bottomI1); #endif #ifdef HAVE_BOTTOM_CAN2 SmartDashboard::PutNumber("Bottom Current 2", bottomI2); SmartDashboard::PutNumber("Bottom Jag ", bottomJagSpeed); #endif SmartDashboard::PutNumber("Bottom Tach ", bottomTachSpeed); // Get setpoint bottomSpeed = SmartDashboard::GetNumber("Bottom Set "); //t2 = GetFPGATime(); if (spinFastNow) { if (bottomPID) { if (bottomJagSpeed < bottomSpeed * vbusThreshold) { bottomPID = false; // below threshold: switch both motors to full output #ifdef HAVE_BOTTOM_CAN1 jagVbus(bottomWheel1, maxOutput); Log(LOG_MODE, 3, 1); #endif #ifdef HAVE_BOTTOM_PWM1 bottomWheel1->Set(maxOutput); Log(LOG_MODE, 3, 1); #endif #ifdef HAVE_BOTTOM_CAN2 jagVbus(bottomWheel2, maxOutput); Log(LOG_MODE, 4, 1); #endif } else { ; // above threshold: run motor 1 off, PID on motor 2 #ifdef HAVE_BOTTOM_CAN1 bottomWheel1->Set(0.0); #endif #ifdef HAVE_BOTTOM_PWM1 bottomWheel1->Set(0.0); #endif #ifdef HAVE_BOTTOM_CAN2 bottomWheel2->Set(bottomSpeed); #endif } } else { if (bottomJagSpeed >= bottomSpeed * pidThreshold) { // above threshold: switch motor 1 off, motor 2 PID bottomPID = true; #ifdef HAVE_BOTTOM_CAN1 bottomWheel1->Set(0.0); #endif #ifdef HAVE_BOTTOM_PWM1 bottomWheel1->Set(0.0); #endif #ifdef HAVE_BOTTOM_CAN2 jagPID(bottomWheel2, bottomSpeed); Log(LOG_MODE, 4, 2); #endif } else { ; // below threshold: run both motors at full output #ifdef HAVE_BOTTOM_CAN1 bottomWheel1->Set(maxOutput); #endif #ifdef HAVE_BOTTOM_PWM1 bottomWheel1->Set(maxOutput); #endif #ifdef HAVE_BOTTOM_CAN2 bottomWheel2->Set(maxOutput); #endif } } //t3 = GetFPGATime(); //printf("%10u %10u %10u\n", (uint32_t)(t1 - t0), (uint32_t)(t2 - t1), (uint32_t)(t3 - t2)); } #endif break; } }