void TeleopPeriodic(void)
{
myarm->prepareSignal();
// Call the drive routine to drive the robot.
if(rightStick->GetRawButton(1)|| leftStick->GetRawButton(1))
drive->MecanumDrive_Cartesian(rightStick->GetX()/2,leftStick->GetY()/2,leftStick->GetX()/2,0.00);
else
drive->MecanumDrive_Cartesian(rightStick->GetX(),leftStick->GetY(),leftStick->GetX(),0.00);
GetStateForArm();
//Wait(.1);
if(modeArm==DDCArm::kManualOveride)
{
myarm->OperateArm(0,0,0,modeArm);
//Shoulder movement
if(leftStick->GetRawButton(3))
myarm->MoveShoulder(1);
else if(leftStick->GetRawButton(2))
myarm->MoveShoulder(-1);
else
myarm->MoveShoulder(0);
//Elbow Movement
if(rightStick->GetRawButton(3))
myarm->MoveElbow(1);
else if (rightStick->GetRawButton(2))
myarm->MoveElbow(-1);
else
myarm->MoveElbow(0);
//Wrist Movement
if(rightStick->GetRawButton(4))
myarm->MoveWrist(-1);
else if(rightStick->GetRawButton(5))
myarm->MoveWrist(1);
else
myarm->MoveWrist(0);
}
else
myarm->OperateArm(0.0,0.0,peg,modeArm);
if(leftStick->GetRawButton(4))
myarm->MoveClaw(-1);
else if(leftStick->GetRawButton(5))
myarm->MoveClaw(1);
else
myarm->MoveClaw(0);
if(rightStick->GetRawButton(10))
{
printf("S: %f \n E: %f \n W: %f \n \n",myarm->GetShoulderVoltage(),myarm->GetElbowVoltage(), myarm->GetWristVoltage());
}
deploy->OperateDeployment(fire,pull);
// Send Data to the Driver Station for Monitoring (w/in .
//sendIOPortData();
//Wait(.1);
}
/*
* Sample line tracking class for FIRST 2011 Competition
* Jumpers on driver station digital I/O pins select the operating mode:
* The Driver Station digital input 1 select whether the code tracks the straight
* line or the forked line. Driver station digital input 2 selects whether the
* code takes the left or right fork. You can set these inputs using jumpers on
* the USB I/O module or in the driver station I/O Configuration pane (if there
* is no Digital I/O module installed.
*
* Since there is the fork to contend with, the code tracks the edge of the line
* using a technique similar to that used with a single-sensor Lego robot.
*
* The two places to do tuning are:
*
* defaultSteeringGain - this is the amount of turning correction applied
* forkProfile & straightProfile - these are power profiles applied at various
* times (one power setting / second of travel) as the robot moves towards
* the wall.
*/
void AutonomousPeriodic(void)
{
// // loop until either we hit the "T" at the end or 8 seconds has
// // elapsed. The time to the end should be less than 7 seconds
// // for either path.
// time = autotimer->Get();
// //if(time < 8.0 && !atCross) {
// if(!atCross){
//
// int timeInSeconds = (int) time;
// int leftValue = left->Get() ? 0 : 1; // read the line tracking sensors
// int middleValue = middle->Get() ? 0 : 1;
// int rightValue = right->Get() ? 0 : 1;
//
// // compute the single value from the 3 sensors. Notice that the bits
// // for the outside sensors are flipped depending on left or right
// // fork. Also the sign of the steering direction is different for left/right.
// if (goLeft)//on fork and go left
// {
// binaryValue = leftValue * 4 + middleValue * 2 + rightValue;
// steeringGain = -defaultSteeringGain;
// }
// else //on straight, or on fork and go right
// {
// binaryValue = rightValue * 4 + middleValue * 2 + leftValue;
// steeringGain = defaultSteeringGain;
// }
// speed=-.2;
// turn = 0; // default to no turn
//
// switch (binaryValue) {
// case 1: // just the outside sensor - drive straight
// turn = 0;
// break;
// case 7: // all sensors - maybe at the "T"
// if (time > stopTime) {
// atCross = true;
// speed = 0;
// }
// break;
// case 0: // no sensors - apply previous correction
// if (previousValue == 0 || previousValue == 1) {
// turn = steeringGain;
// }
// else {
// turn = -steeringGain;
// }
// break;
// default: // anything else, steer back to the line
// turn = -steeringGain;
// }
//
//
// // useful debugging output for tuning your power profile and steering gain
// if(binaryValue != previousValue)
// {
// printf("Time: %2.2f sensor: %d speed: %1.2f turn: %1.2f atCross: %d\n", time, binaryValue, speed, turn, atCross);
// }
//
// // move the robot forward
// float angle = mygyro->GetAngle(); // get heading
// drive->MecanumDrive_Cartesian(turn, speed, 0, angle);
//
// if (binaryValue != 0)
// previousValue = binaryValue;
//
// oldTimeInSeconds = timeInSeconds;
// }
// // stop driving when finished
// else
// drive->MecanumDrive_Cartesian(0,0,0,mygyro->GetAngle());
// Wait(.1);
drive->MecanumDrive_Cartesian(0,0,0,0);
deploy->OperateDeployment(false,true);
}
void DisabledPeriodic(void)
{
// increment the number of disabled periodic loops completed
deploy->OperateDeployment(false,true);
}