void TeleopPeriodic()
{
if(!isWait)
{
//drive
drive->drive(oi->joyDrive->GetRawAxis(FORWARD_Y_AXIS)), setTurnSpeed(oi->joyDrive->GetRawAxis(TURN_X_AXIS));
drive->shift(oi->joyDrive->GetRawButton(8), oi->joyDrive->GetRawButton(9));
//manipulator
manip->moveArm(oi->joyManip->GetRawButton(6), oi->joyManip->GetRawButton(7));
manip->intakeBall(oi->joyManip->GetRawButton(INTAKE_BUTTON), oi->joyManip->GetRawButton(OUTTAKE_BUTTON), (oi->getManipJoystick()->GetThrottle()+1)/2);
catapult->launchBall();
manip->toggleCompressor(oi->joyDrive->GetRawButton(COMPRESSOR_BUTTON));
}
//camera motor mount
if(oi->joyManip->GetRawButton(10))
{
bCameraLatch = true;
}
else if(oi->joyManip->GetRawButton(11))
{
bCameraLatch = false;
}
manip->toggleCameraPosition(bCameraLatch);
}
/**
Tests that the encoder measurements are being correctly translated into
wheel speeds
*/
void tests::measurements() {
Drive md;
EncoderMeasurement measure(md);
md.setMLSpeed(400);
md.setMRSpeed(-400);
uint32_t prevTime;
int i =0;
while(true) {
uint32_t currTime = micros();
if (currTime - prevTime >= PERIOD_MICROS) {
prevTime = currTime;
measure.update();
i++;
if(i % 5 == 0) {
Serial.print(prevTime);
Serial.print("\t");
Serial.print(measure.mVR);
Serial.print("\t");
Serial.println(measure.mVL);
}
}
}
}
Drive* DriveManager::getDrive(std::string type, int index)
{
Drive* drive = createDrive(type, index);
drive->updateFreeSpace();
drive->doQuickSMARTCheck();
return drive;
}
Robot() {
log = new Log(this);
bcd = new BcdSwitch(new DigitalInput(2,11), new DigitalInput(2,12),
new DigitalInput(2,13), new DigitalInput(2,14));
bcdValue = bcd->value();
control = new Control(
new Joystick(1), new Joystick(2), new Joystick(3),
Control::Tank, log);
control->setLeftScale(-1);
control->setRightScale(-1);
control->setGamepadScale(-1);
drive = new Drive(this, 1,2);
drive->addMotor(Drive::Left, 2, 1);
drive->addMotor(Drive::Left, 3, 1);
drive->addMotor(Drive::Right, 4, 1);
drive->addMotor(Drive::Right, 5, 1);
rightDriveEncoder = new Encoder(1,1, 1,2, true, Encoder::k2X);
leftDriveEncoder = new Encoder(1,3, 1,4, false, Encoder::k2X);
//compressor = new Compressor(2,9, 2,3); //press, relay
//compressor->Start();
leftClimber = new Climber(
this,
"leftClimber",
new CANJaguar(6),
new Encoder(1,5, 1,6, true),
ClimberDistancePerPulse,
new DigitalInput(2,1),//lower limit switch
new DigitalInput(2,2),//lower hook switch
new DigitalInput(2,3) );//upper hook switch
rightClimber = new Climber(
this,
"rightClimber",
new CANJaguar(7),
new Encoder(1,7, 1,8, true),
ClimberDistancePerPulse,
new DigitalInput(2,4),//lower limit switch
new DigitalInput(2,5),//lower hook switch
new DigitalInput(2,6) );//upper hook switch
loaderMotor = new Relay(2,1);
loaderSwitch = new DigitalInput(2,8);
shooterMotor = new CANJaguar(10, CANJaguar::kVoltage);
blowerMotor = new CANJaguar(9);
cameraPivotMotor = new Servo(1,9);
cameraElevateMotor = new Servo(1,10);
lightRing = new Relay(2,4);
}
//
// Main Tele Operator Mode function. This function is called once, therefore a while loop that checks IsOperatorControl and IsEnabled is used
// to maintain control until the end of tele operator mode
//
void OperatorControl()
{
//myRobot.SetSafetyEnabled(true);
timer->Start();
Relay* reddlight = new Relay(4);
//Timer* lighttimer = new Timer();
//lighttimer->Start();
while (IsOperatorControl() && IsEnabled())
{
reddlight->Set(reddlight->kForward);
/*
if (lighttimer->Get()<=0.5) {
reddlight->Set(reddlight->kForward);
}
else if(lighttimer->Get()<1){
reddlight->Set(reddlight->kOff);
}
else {
lighttimer->Reset();
}
*/
//
// Get inputs
//
driverInput->GetInputs();
drive->GetInputs();
catapult->GetInputs();
feeder->GetInputs();
//
// Pass values between components as necessary
//
//catapult->SetSafeToFire(feeder->GetAngle()<95);
//
// Execute one step on each component
//
drive->ExecStep();
catapult->ExecStep();
feeder->ExecStep();
//
// Set Outputs on all components
//
catapult->SetOutputs();
feeder->SetOutputs();
//
// Wait for step timer to expire. This allows us to control the amount of time each step takes. Afterwards, restart the
// timer for the next loop
//
while (timer->Get()<(PERIOD_IN_SECONDS));
timer->Reset();
}
}
void TeleopPeriodic() {
// Comment the next line out to disable movement
drive->doDrive(stick->GetX(), -stick->GetY());
intake->periodic();
shooter->periodic();
camSystem->periodic();
}
void tests::servo_drive_interaction() {
VS11 servo(3);
servo.setGoal(0);
delay(1000);
servo.setGoal(M_PI/2);
delay(1000);
Serial.println("0");
{
Drive d;
d.setMLSpeed(100);
d.setMRSpeed(100);
delay(1000);
servo.setGoal(0);
delay(1000);
d.setMLSpeed(-100);
d.setMRSpeed(-100);
delay(1000);
servo.setGoal(M_PI/2);
delay(1000);
d.setMLSpeed(0);
d.setMRSpeed(0);
}
}
bool Agent::fromBottle(Bottle b)
{
if (!this->Object::fromBottle(b))
return false;
if (!b.check("belief")||!b.check("emotions"))
return false;
m_belief.clear();
Bottle* beliefs = b.find("belief").asList();
for(int i=0; i<beliefs->size() ; i++)
{
Bottle* bRelation = beliefs->get(i).asList();
Relation r(*bRelation);
m_belief.push_back(r);
}
m_emotions_intrinsic.clear();
Bottle* emotions = b.find("emotions").asList();
for(int i=0; i<emotions->size() ; i++)
{
Bottle* bEmo = emotions->get(i).asList();
string emotionName = bEmo->get(0).asString().c_str();
double emotionValue = bEmo->get(1).asDouble();
m_emotions_intrinsic[emotionName.c_str()] = emotionValue;
}
m_drives.clear();
Bottle* drivesProperty = b.find("drives").asList();
string drivesDebug = drivesProperty->toString().c_str();
for(int i=0; i<drivesProperty->size() ; i++)
{
Bottle* bD = drivesProperty->get(i).asList();
string drivesDebug1 = bD->toString().c_str();
Drive currentDrive;
currentDrive.fromBottle(*bD);
m_drives[currentDrive.name] = currentDrive;
}
Bottle* bodyProperty = b.find("body").asList();
m_body.fromBottle(*bodyProperty);
return true;
}
const std::string PncKeyGenerator::generate_key(const Drive& drive) {
const auto drive_serial_number = drive.get_fru_info().get_serial_number();
if (!drive_serial_number.has_value()) {
throw KeyValueMissingError("Serial number is missing.");
}
return generate_key_base(drive) + drive_serial_number.value();
}
void AutonomousPeriodic() {
currentDistance = leftDriveEncoder->GetDistance();
shooterMotor->Set(-shooterMotorVolts);
if (currentDistance >= goalDistance){
drive->setLeft(0);
drive->setRight(0);
loaderMotor->Set(Relay::kForward);
} else {
drive->setLeft(.49);
drive->setRight(.5);
}
//log->info("LRD %f %f",
// leftDriveEncoder->GetDistance(),
// rightDriveEncoder->GetDistance());
//log->print();
}
/**
Tests that the encoders work when the motors are spun BY HAND
*/
void tests::encoder_wiring() {
Drive md;
Serial.println("Turn the motors by hand");
uint32_t lastr, lastl;
while(true) {
uint32_t encr = md.getMREncoder();
uint32_t encl = md.getMLEncoder();
if(encr != lastr || encl != lastl) {
Serial.print(encr);
Serial.print(", ");
Serial.print(encl);
Serial.println();
}
delay(10);
lastl = encl;
lastr = encr;
}
}
int main()
{
Drive d;
cout << d.show() <<endl;
cout << d.show_protected_data() <<endl;
Drive *dd = new Drive;
Base *b = dd; //静态是Base动态是Drive
b->v_func(); //结果是d data: 0 指针是所以调用动态函数但是参数是静态 就是base的 0
b->no_virtual();//结果是Base类的函数
Drive_private *pd = new Drive_private;
cout << pd->show_protected_data() <<endl;
//pd->acessy_pubilc_inhirit(); //error 不能被获取
}
/**
Tests that the PID controllers work
*/
void tests::motor_feedback() {
Drive md;
MotorController cont(md);
EncoderMeasurement measure(md);
uint32_t prevTime;
int i =0;
while(true) {
uint32_t currTime = micros();
if (currTime - prevTime >= PERIOD_MICROS) {
prevTime = currTime;
measure.update();
cont.controlMR(0.1, measure.mVR); // right motor PI control
cont.controlML(-0.1, measure.mVL); //left motor PI control
i++;
if(i % 100 == 0) {
Serial.print(measure.encoderRCount);
Serial.print(", ");
Serial.println(measure.encoderLCount);
Serial.print(": ");
Serial.print(measure.mVR, 4);
Serial.print(", ");
Serial.println(measure.mVL, 4);
}
if(md.faulted()) {
Serial.println("Motor fault");
md.setMLSpeed(0);
md.setMRSpeed(0);
delay(100);
md.clearFault();
}
}
}
}
void TeleopPeriodic()
{
manip->toggleCompressor(oi->joyDrive->GetRawButton(COMPRESSOR_BUTTON));
//drive
drive->drive(oi->joyDrive->GetRawAxis(FORWARD_Y_AXIS), oi->joyDrive->GetRawAxis(TURN_X_AXIS));
//drive->shift(oi->joyDrive->GetRawButton(8), oi->joyDrive->GetRawButton(9));
//manipulator
// TJF: Replaced "magic numbers" with named constants
manipArm->moveArm(oi->joyDrive->GetRawButton(UP_INTAKE_BUTTON), oi->joyDrive->GetRawButton(DOWN_INTAKE_BUTTON)); //manipArm->moveArm(oi->joyDrive->GetRawButton(6), oi->joyDrive->GetRawButton(7));
manip->intakeBall(oi->joyDrive->GetRawButton(INTAKE_BUTTON), oi->joyDrive->GetRawButton(OUTTAKE_BUTTON), (oi->joyDrive->GetThrottle()+1)/2);
// TJF: removed only because it doesn't work yet
catapult->launchBall();
printSmartDashboard(); // 01/18/2016 moved this function because it needed to be updated constantly instead of initializing it only once.
}
bool
Changer::LoadCartridge(
Drive & drive,
Cartridge & cartridge,
Error & error)
{
boost::lock_guard<boost::mutex> lock(detail_->mutex);
int slotIDSrc;
if ( ! cartridge.GetSlotID(slotIDSrc,error) ) {
return false;
}
int slotIDDst;
if ( ! drive.GetSlotID(slotIDDst,error) ) {
return false;
}
return detail_->MoveCartridge(slotIDSrc,slotIDDst,cartridge,error);
}
void printSmartDashboard()
{
SmartDashboard::PutNumber("Drive Y-Value", oi->joyDrive->GetRawAxis(FORWARD_Y_AXIS)); //oi->getDashboard()->PutNumber("Drive Y-Value", oi->joyDrive->GetRawAxis(FORWARD_Y_AXIS));
SmartDashboard::PutNumber("Drive X-Value", oi->joyDrive->GetRawAxis(TURN_X_AXIS));//oi->getDashboard()->PutNumber("Drive X-Value", oi->joyDrive->GetRawAxis(TURN_X_AXIS));
SmartDashboard::PutBoolean("Compressor On?", manip->compressorState()); //oi->getDashboard()->PutBoolean("Compressor On?", manip->compressorState());
SmartDashboard::PutNumber("Gyro Value", drive->navX->GetAngle());
//Returns Encoder Position(with tick)
//deleted two front motors because encoder is attached only to the back talons
SmartDashboard::PutNumber("Left Encoder Position", drive->rearLeftMotor->GetEncPosition());
SmartDashboard::PutNumber("Right Encoder Position", drive->rearRightMotor->GetEncPosition());
//Returns how fast the wheel is spinning
//deleted two front motors because encoder is attached only to the back talons
SmartDashboard::PutNumber("Left Encoder Speed", drive->rearLeftMotor->GetEncVel()); //gives value for both back and front left encoder
SmartDashboard::PutNumber("Right Encoder Speed", drive->rearRightMotor->GetEncVel()); //TODO: Does not return right encoder value
SmartDashboard::PutNumber("Average Encoder Value", drive->getAvgEncVal()); //Is working 1/21/2016
}
int _tmain(int argc, _TCHAR* argv[])
{
WindowsDriveManager dm;
Drive* drive = dm.getDrive("USB", 0);
assert(0 == drive->getDriveName().compare("WMI USB"));
assert(0 != drive->getDriveName().compare("WMI default"));
assert(0 != drive->getDriveName().compare("WMI SATA"));
assert(0 != drive->getDriveName().compare("Linux USB"));
assert(0 != drive->getDriveName().compare("Linux default"));
assert(0 != drive->getDriveName().compare("Linux SATA"));
delete drive;
drive = dm.getDrive("SAS", 0);
assert(NULL == drive);
delete drive;
return 0;
}
bool homeostaticModule::addNewDrive(string driveName)
{
cout << "adding a default drive..." << endl;
Drive* drv = new Drive(driveName);
drv->setHomeostasisMin(drv->homeostasisMin);
drv->setHomeostasisMax(drv->homeostasisMax);
drv->setDecay(drv->decay);
drv->setValue((drv->homeostasisMax + drv->homeostasisMin) / 2.);
drv->setGradient(false);
manager.addDrive(drv);
cout << "default drive added. Opening ports..."<<endl;
openPorts(driveName);
cout << "new drive created successfully!"<<endl;
return true;
}
bool homeostaticModule::addNewDrive(string driveName, yarp::os::Bottle& grpHomeostatic)
{
Drive* drv = new Drive(driveName);
drv->setHomeostasisMin(grpHomeostatic.check((driveName + "-homeostasisMin"), Value(drv->homeostasisMin)).asDouble());
drv->setHomeostasisMax(grpHomeostatic.check((driveName + "-homeostasisMax"), Value(drv->homeostasisMax)).asDouble());
cout << "H1 " << grpHomeostatic.check((driveName + "-decay"), Value(drv->decay)).asDouble() <<endl;
drv->setDecay(grpHomeostatic.check((driveName + "-decay"), Value(drv->decay)).asDouble());
drv->setValue((drv->homeostasisMax + drv->homeostasisMin) / 2.);
drv->setGradient(grpHomeostatic.check((driveName + "-gradient"), Value(drv->gradient)).asBool());
cout << "h2 " << drv->decay << endl;
//cout << drv->name << " " << drv->homeostasisMin << " " << drv->homeostasisMax << " " << drv->decay << " " <<drv->gradient << endl;
//cout << d << endl;
//cout << grpHomeostatic.toString()<<endl;
manager.addDrive(drv);
cout << "h2 " << manager.drives[0]->decay << endl;
openPorts(driveName);
return true;
}
void TeleopPeriodic() {
drive->doDrive(input.GetX(), input.GetY(), input.GetRotation());
input.GetGrabberInput();
input.GetArmInput();
}
/**
* Periodic code for autonomous mode should go here.
*
* Use this method for code which will be called periodically at a regular
* rate while the robot is in autonomous mode.
*/
void RobotDemo::AutonomousPeriodic()
{
shooter->handle();
intake->handle();
switch(curAutoState)
{
case AUTO_EXTEND:
if(curAutoState != preAutoState)
{
preAutoState = curAutoState;
intake->move(INTAKE_EXTEND_POWER);
printf("extending, pre %d, cur %d, \n", preAutoState, curAutoState);
}
if(intake->getCurrentPosition() >= EXTENDER_MAX_DISTANCE)
{
intake->move(STOP);
curAutoState = AUTO_DRIVE;
printf("pre %d, cur %d, stopped extender \n", preAutoState, curAutoState);
}
break;
case AUTO_DRIVE:
if(curAutoState != preAutoState)
{
preAutoState = curAutoState;
drive->InitAutoDrive(AUTO_DRIVE_DISTANCE_RIGHT,
AUTO_DRIVE_DISTANCE_LEFT,
AUTO_DRIVE_SPEED);
printf("We're moving! pre %d, cur %d, \n", preAutoState, curAutoState);
}
/* if(HotAutoSensor->Get())
{
HotAutoStarts = true;
}
*/
if(drive->autoDrive() == true)
{
curAutoState = AUTO_STOP_DRIVING;
}
break;
case AUTO_STOP_DRIVING:
if(curAutoState != preAutoState)
{
preAutoState = curAutoState;
printf("now stopping pre %d, cur %d, \n", preAutoState, curAutoState);
drive->TeleopDrive(AUTO_STOPPING_SPEED_LEFT, AUTO_STOPPING_SPEED_RIGHT);
}
if(drive->getSpeed() > 0.01)
{
drive->TeleopDrive(AUTO_STOPPED_SPEED_LEFT, AUTO_STOPPED_SPEED_RIGHT);
// if(HotAutoStarts || HotAutoTimer->Get() > 5.0)
// {
curAutoState = AUTO_SHOOT;
// }
}
break;
case AUTO_SHOOT:
if(curAutoState != preAutoState)
{
preAutoState = curAutoState;
printf("now shooting pre %d, cur %d, \n", preAutoState, curAutoState);
shooter->initShot(SHOT_HIGH_ANGLE, SHOT_HIGH_SPEED);
}
if(!shooter->isShooting())
{
curAutoState = AUTO_STOP;
}
break;
/*
case AUTO_MOBILITY:
if(curAutoState != preAutoState)
{
drive->InitAutoDrive(AUTO_SETUP_SHOT_DISTANCE_RIGHT,
AUTO_SETUP_SHOT_DISTANCE_LEFT,
AUTO_SETUP_SHOT_SPEED);
}
if(drive->autoDrive())
{
curAutoState = AUTO_STOP;
}
break;
*/
case AUTO_STOP:
preAutoState = curAutoState;
printf("auto is done pre %d, cur %d, \n", preAutoState, curAutoState);
break;
// HotAutoTimer->Stop();
default:
break;
}
}
/**
* Periodic code for teleop mode should go here.
*
* Use this method for code which will be called periodically at a regular
* rate while the robot is in teleop mode.
*/
void RobotDemo::TeleopPeriodic()
{
drive->TeleopDrive(driverPad->GetLeftY() *DRIVER_LEFT_DIRECTION, driverPad->GetRightY() *DRIVER_RIGHT_DIRECTION);
shooter->handle();
shooter->debug();
//shooter->setShooterPower(operatorPad->GetRightY());
if((intakeMode == IN_INTAKE_MANUAL_MODE) && operatorPad->GetNumberedButton(INTAKE_AUTO_MODE_BUTTON))
{
intakeMode = IN_INTAKE_AUTO_MODE;
printf ("set into auto roller mode");
intake->setExtenderMode(IN_INTAKE_AUTO_MODE);
}
//sets intake mode to auto if previously in manual and operator presses auto button
else if((intakeMode == IN_INTAKE_AUTO_MODE) && operatorPad->GetNumberedButton(INTAKE_MANUAL_MODE_BUTTON))
{
intakeMode = IN_INTAKE_MANUAL_MODE;
printf ("set into manual roller mode");
intake->setExtenderMode(IN_INTAKE_MANUAL_MODE);
}
intake->handle();
if(intakeMode == IN_INTAKE_MANUAL_MODE)
{
intake->move(operatorPad->GetLeftY()*OPERATOR_LEFT_DIRECTION);
}
else
{
//Extender DPad control
curPadDir = operatorPad->GetDPad();
if(lastPadDir != curPadDir)
{
switch(curPadDir)
{
case Gamepad::kUp:
intake->setExtenderTarget(FULL_EXTEND_DISTANCE);
break;
case Gamepad::kDown:
intake->setExtenderTarget(FULL_RETRACT_DISTANCE);
break;
default:
break;
}
}
lastPadDir = curPadDir;
}
//intake->spinRoller(operatorPad->GetRightY()*OPERATOR_RIGHT_DIRECTION);
intake->getCurrentPosition();
//if operator taps in button roller continually spins inward
if(operatorPad->GetNumberedButton(ROLLER_IN_BUTTON))
{
intakeCont = INTAKE_ROLLER_IN_STATE;
}
//if operator taps stop button roller stops continually
else if(operatorPad->GetNumberedButton(ROLLER_OFF_BUTTON))
{
intakeCont = INTAKE_ROLLER_STOP_STATE;
}
//if operator holds spit button roller spins backwards no matter what
if(operatorPad->GetNumberedButton(ROLLER_OUT_BUTTON))
{
intake->spinRoller(INTAKE_ROLLER_SPIT_POWER);
}
//not holding spit button
else
{
//roller either spins or is stopped depending on previous code
if(intakeCont)
{
intake->spinRoller(INTAKE_ROLLER_IN_POWER);
}
else
{
intake->spinRoller(INTAKE_ROLLER_STOP_POWER);
}
}
if(operatorPad->GetNumberedButton(RIGHT_TRIGGER_BUTTON)) // low shot = Right Trigger
{
shooter->initShot(SHOT_LOW_ANGLE, SHOT_LOW_SPEED);
printf("test button 1 is pressed");
}
else if(operatorPad->GetNumberedButton(LEFT_TRIGGER_BUTTON)) // pass = Left Trigger
{
shooter->initShot(SHOT_PASS_ANGLE, SHOT_PASS_SPEED);
printf("test button 2 is pressed");
}
else if(operatorPad->GetNumberedButton(RIGHT_BUMPER_BUTTON)) // high shot = Right Bumper
{
shooter->initShot(SHOT_HIGH_ANGLE, SHOT_HIGH_SPEED);
printf("test button 3 is pressed");
}
else if(operatorPad->GetNumberedButton(LEFT_BUMPER_BUTTON)) // truss = Left Bumper
{
shooter->initShot(SHOT_TRUSS_ANGLE, SHOT_TRUSS_SPEED);
printf("test button 4 is pressed");
}
//printf("right Joystick distance: %f left Joystick distance: %f \n", driverPad->GetRightY(), driverPad->GetLeftY());
}
int main(int argc, char* argv[]){
Drive d;
d.show();
return 0;
}
void Test()
{
SmartDashboard::init();
Joystick* driverStick = driverInput->GetDriverJoystick();
//Relay* redLights = new Relay(1, Relay::kForward);
//Relay* blueLights = new Relay(2, Relay::kForward);
//Relay* cameraLights = new Relay(CAMERA_LED);
//Relay* redLights = new Relay(1, Relay::kForward);
//Solenoid *rightSolenoid;
//Solenoid *leftSolenoid;
bool gear = false;
bool prevShiftButttonPressed = false;
Victor* motors [] = {feeder->feederWheel, feeder->feederArm, drive->leftDrive, drive->rightDrive, catapult->ChooChooMotor};
Relay* compressor = new Relay(COMPRESSOR_RELAY);
DigitalInput* compressorLimitSwitch = new DigitalInput(COMPRESSOR_SWITCH);
//Compressor* compressor = new Compressor(1,1);//COMPRESSOR_SWITCH, COMPRESSOR_RELAY);
//compressor->Start();
Relay* cameraLights = new Relay(CAMERA_LED);
//Solenoid* right = new Solenoid(6);
//Solenoid* left = new Solenoid(5);
//right->Set(true);
//left->Set(true);
//Relay* redlights = catapult->redled;
//Relay* whitelights = catapult->whiteled;
//redlights->Set(redlights->kForward);
//whitelights->Set(whitelights->kForward);
//cameraLights->Set(cameraLights->kForward);
bool compressorOn = false;
while (IsTest() && IsEnabled())
{
bool curShiftButtonPressed = driverStick->GetRawButton(10);
//sensors->GetInputs();
//SmartDashboard::PutNumber("Rangefinder", sensors->GetDistance());
/*FEEDER WHEEL*/
if (driverStick->GetRawButton(2))
{
motors[0]->Set(0.4);
}
else
{
motors[0]->Set(0);
}
/*FEEDER ARM*/
if (driverStick->GetRawButton(3))
{
motors[1]->Set(-0.6);
}
else
{
motors[1]->Set(0);
}
/*LEFT DRIVE*/
if (driverStick->GetRawButton(4))
{
motors[2]->Set(0.8);
}
else
{
motors[2]->Set(0);
}
/*RIGHT DRIVE*/
if (driverStick->GetRawButton(5))
{
motors[3]->Set(0.8);
}
else
{
motors[3]->Set(0);
}
/*
if (driverStick->GetRawButton(6))
{
redlights->Set(redlights->kForward);
}
else
{
redlights->Set(redlights->kOff);
}
*/
/*CHOOCHOO MOTOR*/
if (driverStick->GetRawButton(7) && driverStick->GetRawButton(1))
{
motors[4]->Set(0.6);
}
else
{
motors[4]->Set(0);
}
/*CAMERA LED*/
if (driverStick->GetRawButton(8))
{
cameraLights->Set(cameraLights->kForward);
}
else
{
cameraLights->Set(cameraLights->kOff);
}
/*
if (curShiftButtonPressed && !prevShiftButttonPressed)
{
drive->SetHighGear(!gear);
}
*/
if (driverStick->GetRawButton(10))
{
drive->SetHighGear(true);//!gear
gear = !gear;
}
else{
drive->SetHighGear(false);
}
/*COMPRESSOR*/
if(driverStick->GetRawButton(11))
{
compressorOn = true;
//compressor->Set(compressor->kOn);
//compressor->Start();
}
else
{
//compressor->Stop();
}
if(driverStick->GetRawButton(9))
{
compressorOn = false;
//compressor->Set(compressor->kOff);
//compressor->Stop();
}
if(compressorOn){
if(!compressorLimitSwitch->Get()){
compressor->Set(compressor->kOn);
}
else{
compressor->Set(compressor->kOff);
compressorOn = false;
}
}
else
compressor->Set(compressor->kOff);
prevShiftButttonPressed = curShiftButtonPressed;
}
}
/**
Tests that the motors are wired up correctly.
When running, verify that the motor and direction printed over serial is what
is observed on the robot.
Also prints the change in encoder values after each movement - expect the
readings to positive, and correspond with the motor that moved
*/
void tests::motor_wiring() {
Drive md;
md.resetEncoders();
if(md.faulted()) {
Serial.println("Motor driver has a fault");
md.clearFault();
if(md.faulted()) {
Serial.println("Motor driver still has a fault");
return;
}
}
else
Serial.println("Motors are ok");
{
Serial.print("Motor R, forward: ");
uint32_t encr = md.getMREncoder();
uint32_t encl = md.getMLEncoder();
Serial.flush();
md.setMRSpeed(100);
delay(2e3);
md.setMRSpeed(0);
int32_t diffr = md.getMREncoder() - encr;
int32_t diffl = md.getMLEncoder() - encl;
Serial.print("dr =");
Serial.print(diffr);
Serial.print(", dl =");
Serial.println(diffl);
}
if(md.faulted())
Serial.println("Motor driver has a fault");
{
Serial.print("Motor L, forward: ");
uint32_t encr = md.getMREncoder();
uint32_t encl = md.getMLEncoder();
Serial.flush();
md.setMLSpeed(100);
delay(2e3);
md.setMLSpeed(0);
int32_t diffr = md.getMREncoder() - encr;
int32_t diffl = md.getMLEncoder() - encl;
Serial.print("dr =");
Serial.print(diffr);
Serial.print(", dl =");
Serial.println(diffl);
}
if(md.faulted())
Serial.println("Motor driver has a fault");
}
void TeleopInit() {
init();
drive->setShiftMode(Drive::Manual);
shooterMotor->Set(0.0);
loaderMotor->Set(Relay::kOff);
}
void TeleopPeriodic() {
displayCount++;
//tbs change button number
if ((control->gamepadButton(9) && control->gamepadButton(10)) || // start
climbState != NotInitialized) { // continue
// Do we want a manual abort here?
ClimbPeriodic();
return;
}
// drive
drive->setLeft(control->left());
// control->setRightScale(.95);
drive->setRight(control->right());
drive->setScale(control->throttle());
//drive->setLowShift(control->button(1)); // right trigger
drive->setReversed(control->toggleButton(11)); // right JS button 11
//turn light on or off
//lightRing->Set(control->gamepadToggleButton(4) ? Relay::kForward : Relay::kOff );
blowerMotor->Set(control->gamepadButton(6) ? 1.0 : 0.0 );
// For the loader, if we are rotating, wait for at least 100 counts before
// checking the switch or adjusting motor power
if (loading) {
//if (loaderSwitch->Get()) {
//loaderDisengageDetected = true;
//}
loadSwitchDelay++;
// If already rotating, and the switch trips, power down the motor
if (/*loaderDisengageDetected*/ loaderSwitch->Get() != loadSwitchOldState) {
loaderMotor->Set(Relay::kOff);
loading = false;
loadSwitchOldState = loaderSwitch->Get();
}
} else {
// If not rotating and the gamepad button is set, start rotating
if (control->gamepadButton(7)) {
loadSwitchDelay = 0;
loadCount++;
loaderMotor->Set(Relay::kForward);
loading = true;
loaderDisengageDetected = false;
}
}
if (control->gamepadToggleButton(4)){
if (control->gamepadRightVertical() > 0.05 ||
control->gamepadRightVertical() < -0.05) {
shooterMotorVolts += control->gamepadRightVertical();
if (shooterMotorVolts < 6.0)
shooterMotorVolts = 6.0;
if (shooterMotorVolts > 12.0)
shooterMotorVolts = 12.0;
}
}
// For the shooter, spin it up or down based on the toggle
//
if (control->gamepadToggleButton(8)) {
shooterMotor->Set(-shooterMotorVolts);// negative because motor is wired backwerds
log->info("shooter on");
} else {
shooterMotor->Set(0.0);
log->info("shooter off");
}
if (control->gamepadLeftVertical() > 0.05 ||
control->gamepadLeftVertical() < -0.05) {
cameraElevateAngle += control->gamepadLeftVertical()*5;
if (cameraElevateAngle < cameraElevateMotor->GetMinAngle())
cameraElevateAngle = cameraElevateMotor->GetMinAngle();
if (cameraElevateAngle > cameraElevateMotor->GetMaxAngle())
cameraElevateAngle = cameraElevateMotor->GetMaxAngle();
}
if (control->gamepadLeftHorizontal() > 0.05 ||
control->gamepadLeftHorizontal() < -0.05) {
cameraPivotAngle += control->gamepadLeftHorizontal()*5;
if (cameraPivotAngle < cameraPivotMotor->GetMinAngle())
cameraPivotAngle = cameraPivotMotor->GetMinAngle();
if (cameraPivotAngle > cameraPivotMotor->GetMaxAngle())
cameraPivotAngle = cameraPivotMotor->GetMaxAngle();
}
cameraPivotMotor->SetAngle(cameraPivotAngle);
cameraElevateMotor->SetAngle(cameraElevateAngle);
if (control->gamepadToggleButton(1)) {
// If the climber lower limit switch is set, and the distance is >0, set it to 0
if (!leftClimber->lowerLimitSwitch->Get()) {
leftClimber->encoder->Reset();
leftClimber->encoder->Start();
// Only allow forward motion
if (control->gamepadRightVertical() > 0) {
leftClimber->motor->Set(control->gamepadRightVertical());
} else {
leftClimber->motor->Set(0.0);
}
} else {
leftClimber->motor->Set(control->gamepadRightVertical());
}
}
if (control->gamepadToggleButton(3)) {
// If the climber lower limit switch is set, and the distance is >0, set it to 0
if (!rightClimber->lowerLimitSwitch->Get()) {
rightClimber->encoder->Reset();
rightClimber->encoder->Start();
// Only allow forward motion
if (control->gamepadRightVertical() > 0) {
rightClimber->motor->Set(control->gamepadRightVertical());
} else {
rightClimber->motor->Set(0.0);
}
} else {
rightClimber->motor->Set(control->gamepadRightVertical());
}
}
// assorted debug
//log->info("Shift %s", control->toggleButton(8)
// ? "low" : "high");
//log->info("ArmPot: %.0f", arm->encoderValue());
//log->info("pidf: %.2f", arm->pidFactor());
//log->info("piden: %s", arm->isPidEnabled() ? "true" : "false");
//double myTest = drive->rightPosition();
//log->info("renc: %f", myTest);
//myTest = drive->leftPosition();
//log->info("lenc: %f", myTest);
//log->info("gplh %f %f", control->gamepadLeftHorizontal(), cameraPivotAngle);
//log->info("gplv %f %f", control->gamepadLeftVertical(), cameraElevateAngle);
//log->info("gprh %f", control->gamepadRightHorizontal());
//log->info("gprv %f", control->gamepadRightVertical());
if (display()) {
//log->info("lg %d ldd %d", loading, loaderDisengageDetected);
//log->info("BCD: %d", bcd->value());
log->info("SHV: %f", shooterMotorVolts);
//log->info("LdCDS: %d %d %d", loadCount, loaderDisengageDetected, loaderSwitch->Get());
log->info("ena: %c%c%c%c",
" L"[control->gamepadToggleButton(1)],
" R"[control->gamepadToggleButton(3)],
" J"[control->gamepadToggleButton(2)],
" S"[control->gamepadToggleButton(4)]);
log->info("LRC %f %f",
leftClimber->encoder->GetDistance(),
rightClimber->encoder->GetDistance());
log->info("LRD %f %f",
leftDriveEncoder->GetDistance(),
rightDriveEncoder->GetDistance());
log->info("LRL %d %d %d %d %d %d %d",
leftClimber->lowerLimitSwitch->Get(),
leftClimber->lowerHookSwitch->Get(),
leftClimber->upperHookSwitch->Get(),
rightClimber->lowerLimitSwitch->Get(),
rightClimber->lowerHookSwitch->Get(),
rightClimber->upperHookSwitch->Get(),
loaderSwitch->Get());
log->print();
}
}
