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;
}
}