/**
* Return the actual angle in degrees that the robot is currently facing.
*
* The angle is based on the current accumulator value corrected by the oversampling rate, the
* gyro type and the A/D calibration values.
* The angle is continuous, that is can go beyond 360 degrees. This make algorithms that wouldn't
* want to see a discontinuity in the gyro output as it sweeps past 0 on the second time around.
*
* @param slot The slot the analog module is connected to
* @param channel The analog channel the gyro is plugged into
* @return the current heading of the robot in degrees. This heading is based on integration
* of the returned rate from the gyro.
*/
float GetGyroAngle(UINT32 slot, UINT32 channel)
{
Gyro *gyro = AllocateGyro(slot, channel);
if (gyro)
return gyro->GetAngle();
return 0.0;
}
void OperatorControl()
{
float xJoy, yJoy, gyroVal, angle = 0, turn = 0, angleDiff, turnPower;
gyro.Reset();
gyro.SetSensitivity(9.7);
while (IsEnabled() && IsOperatorControl()) // loop as long as the robot is running
{
yJoy = xbox.getAxis(Xbox::L_STICK_V);
xJoy = xbox.getAxis(Xbox::R_STICK_H);
gyroVal = gyro.GetAngle()/0.242*360;
turn = 0.15;
angle = angle - xJoy * xJoy * xJoy * turn;
angleDiff = mod(angle - gyroVal, 360);
turnPower = - mod(angleDiff / 180.0 + 1.0, 2) + 1.0;
SmartDashboard::PutString("Joy1", vectorToString(xJoy, yJoy));
SmartDashboard::PutNumber("Heading", mod(gyroVal, 360));
SmartDashboard::PutNumber("Turn Power", turnPower);
SmartDashboard::PutBoolean("Switch is ON:", dumperSwitch.Get());
if (!xbox.isButtonPressed(Xbox::R))
{
drive.ArcadeDrive(yJoy * yJoy * yJoy, turnPower * fabs(turnPower), false);
}
}
}
void DemoSceneManager::draw(double deltaT)
{
_time += deltaT;
float angle = _time * .1; // .1 radians per second
glClearColor(0.25f, 0.25f, 0.25f, 1.0f);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LEQUAL);
glCullFace(GL_BACK);
glEnable(GL_CULL_FACE);
Gyro *gyro = Gyro::getInstance();
gyro->read();
vmml::mat4f translation = vmml::create_translation(vmml::vec3f(_scrolling.x(), -_scrolling.y(), 0));
vmml::mat4f scaling = vmml::create_scaling(vmml::vec3f(.2f));
vmml::mat3f rotation = vmml::create_rotation(gyro->getRoll() * -M_PI_F - .3f, vmml::vec3f::UNIT_Y) *
vmml::create_rotation(gyro->getPitch() * -M_PI_F + .3f, vmml::vec3f::UNIT_X);
_eyePos = rotation * vmml::vec3f(0.0f, 0.0f, 0.0f);
vmml::vec3f eyeUp = vmml::vec3f::DOWN;
_viewMatrix = lookAt(_eyePos, vmml::vec3f::UP, rotation * eyeUp);
_modelMatrix = translation * scaling;
// drawModel2("quad");
drawModel("tunnel3");
// drawModel2("quad");
_eyePos = rotation * vmml::vec3f(0.0f, 2.0f, 0.0f);
_viewMatrix = lookAt(_eyePos, vmml::vec3f::UP, rotation * eyeUp);
}
/**
* Set the gyro type based on the sensitivity.
* This takes the number of volts/degree/second sensitivity of the gyro and uses it in subsequent
* calculations to allow the code to work with multiple gyros.
*
* @param slot The slot the analog module is connected to
* @param channel The analog channel the gyro is plugged into
* @param voltsPerDegreePerSecond The type of gyro specified as the voltage that represents one degree/second.
*/
void SetGyroSensitivity(UINT32 slot, UINT32 channel,
float voltsPerDegreePerSecond)
{
Gyro *gyro = AllocateGyro(slot, channel);
if (gyro)
gyro->SetSensitivity(voltsPerDegreePerSecond);
}
/**
* Initialization code for autonomous mode should go here.
*
* Use this method for initialization code which will be called each time
* the robot enters autonomous mode.
*/
void RA14Robot::AutonomousInit() {
Config::LoadFromFile("config.txt");
auto_case = (int) Config::GetSetting("auto_case", 1);
alreadyInitialized = true;
auto_timer->Reset();
auto_timer->Start();
missionTimer->Start();
myDrive->ResetOdometer();
myCamera->Set(Relay::kForward);
myCollection->ExtendArm();
cout<<"Reseting Gyro"<<endl;
gyro->Reset();
//myOdometer->Reset();
//myDrive->ShiftUp();
myDrive->ShiftDown();
//shift to high gear
if (!fout.is_open()) {
cout << "Opening logging.csv..." << endl;
fout.open("logging.csv");
logheaders();
}
auto_state = 0;
#ifndef DISABLE_SHOOTER
myCam->Reset();
myCam->Enable();
#endif //Ends DISABLE_SHOOTER
}
RobotDemo()
{
//Initialize the objects for the drive train
myRobot = new RobotDrive(1, 2);
leftEnco = new Encoder(LEFT_ENCO_PORT_1, LEFT_ENCO_PORT_2);
rightEnco = new Encoder(RIGHT_ENCO_PORT_1, RIGHT_ENCO_PORT_2);
leftEnco->SetDistancePerPulse((4 * 3.14159)/ENCO_PULSES_PER_REV);
leftEnco->Start();
rightEnco->SetDistancePerPulse((4 * 3.14159)/ENCO_PULSES_PER_REV);
rightEnco->Start();
leftStick = new Joystick(1);
rightStick = new Joystick(2);
//Initialize the gyro
gyro = new Gyro(GYRO_PORT);
gyro->Reset();
//Initialize the manipulators
intake = new Intake(INTAKE_ROLLER_PORT, BALL_SENSOR_PORT, LEFT_SERVO_PORT, RIGHT_SERVO_PORT);
catapult = new Catapult(LOADING_MOTOR_PORT, HOLDING_MOTOR_PORT, LOADED_LIMIT_1_PORT,
LOADED_LIMIT_2_PORT, HOLDING_LIMIT_PORT);
//Initialize the objects needed for camera tracking
rpi = new RaspberryPi("17140");
LEDLight = new Relay(1);
LEDLight->Set(Relay::kForward);
//Set the autonomous modes
autonMode = ONE_BALL_AUTON;
autonStep = AUTON_ONE_SHOOT;
//Initialize the lcd
lcd = DriverStationLCD::GetInstance();
}
/**
* This autonomous (along with the chooser code above) shows how to select between different autonomous modes
* using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
* Dashboard, remove all of the chooser code and uncomment the GetString line to get the auto name from the text box
* below the Gyro
*
* You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings.
* If using the SendableChooser make sure to add them to the chooser code above as well.
*/
void AutonomousInit()
{
autoSelected = *((std::string*)chooser->GetSelected());
//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
std::cout << "Auto selected: " << autoSelected << std::endl;
rotation = 0.0;
//*((double*)posChooser->GetSelected());
//goal = *((std::string*)goalChooser->GetSelected());
shoot = "No";
//*((std::string*)shootChooser->GetSelected());
defenseCrossed = false;
done = false;
std::cout << "Here" << std::endl;
drive->SetMaxOutput(1.0);
std::cout << "there" << std::endl;
//Make sure to reset the encoder!
leftEnc->Reset();
rightEnc->Reset();
gyro->Reset();
autoCounter = 0;
timer->Reset();
}
//robot turns to desired position with a deadband of 2 degrees in each direction
bool GyroTurn (float desiredTurnAngle, float turnSpeed)
{
bool turning = true;
float myAngle = gyro->GetAngle();
//normalizes angle from gyro to [-180,180) with zero as straight ahead
while(myAngle >= 180)
{
GetWatchdog().Feed();
myAngle = myAngle - 360;
}
while(myAngle < -180)
{
GetWatchdog().Feed();
myAngle = myAngle + 360;
}
if(myAngle < (desiredTurnAngle - 2))// if robot is too far left, turn right a bit
{
myRobot->Drive(turnSpeed, -turnSpeed); //(right,left)
}
if(myAngle > (desiredTurnAngle + 2))// if robot is too far right, turn left a bit
{
myRobot->Drive(-turnSpeed, turnSpeed); //(right,left)
}
else
{
myRobot->Drive(0, 0);
turning = false;
}
return turning;
}
void TeleopInit()
{
leftEnc->Reset();
rightEnc->Reset();
gyro->Reset();
powerCounter = 0;
}
virtual void SetUp() override {
m_tilt = new Servo(TestBench::kCameraTiltChannel);
m_pan = new Servo(TestBench::kCameraPanChannel);
m_spiAccel = new ADXL345_SPI(SPI::kOnboardCS0);
m_tilt->Set(kTiltSetpoint45);
m_pan->SetAngle(0.0f);
Wait(kServoResetTime);
m_gyro->Reset();
}
void Test() {
robotDrive.SetSafetyEnabled(false);
uint8_t toSend[10];//array of bytes to send over I2C
uint8_t toReceive[10];//array of bytes to receive over I2C
uint8_t numToSend = 1;//number of bytes to send
uint8_t numToReceive = 0;//number of bytes to receive
toSend[0] = 7; //send 0 to arduino
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
bool isSettingUp = true;
pickup.setGrabber(-1);
pickup.setLifter(1);
while (isSettingUp) {
isSettingUp = false;
if (grabOuterLimit.Get() == false) {
pickup.setGrabber(0);
}
else {
isSettingUp = true;
}
if (liftLowerLimit.Get()) {
pickup.setLifter(0);
}
else {
isSettingUp = true;
}
}
gyro.Reset();
liftEncoder.Reset();
grabEncoder.Reset();
toSend[0] = 8;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
while(IsTest() && IsEnabled());
toSend[0] = 0;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
}
bool GyroDrive(float desiredDriveAngle, float speed, int desiredDistance)
{
bool driving = true;
double encoderInchesTraveled = fabs(leftEnco->GetDistance());//absolute value distance
float myAngle = gyro->GetAngle();
//normalizes angle from gyro to [-180,180) with zero as straight ahead
while(myAngle >= 180)
{
GetWatchdog().Feed();
myAngle = myAngle - 360;
}
while(myAngle < -180)
{
GetWatchdog().Feed();
myAngle = myAngle + 360;
}
float my_speed = 0.0;
float turn = 0.0;
if(speed > 0)
//30.0 is the number you have to change to adjust properly
turn = -((myAngle + desiredDriveAngle) / 30.0); //proportionally adjust turn. As the robot gets more off 0, the greater the turn will be
else
turn = (myAngle + desiredDriveAngle) / 30.0; //proportionally adjust turn. As the robot gets more off 0, the greater the turn will be
if (encoderInchesTraveled < desiredDistance)
my_speed = speed;
else
{
my_speed = 0.0;
driving = false;
}
myRobot->Drive(my_speed, turn);
return driving;
}
void RA14Robot::logging() {
if (fout.is_open()) {
fout << missionTimer->Get() << ",";
#ifndef DISABLE_SHOOTER
myCam->log(fout);
#endif //Ends DISABLE_SHOOTER
myDrive->log(fout);
CurrentSensorSlot * slots[4] = { camMotor1Slot, camMotor2Slot,
driveLeftSlot, driveRightSlot };
DriverStation * ds = DriverStation::GetInstance();
for (int i = 0; i < 4; ++i) {
fout << slots[i]->Get() << ",";
}
fout << auto_case << "," << gyro->GetAngle() << "," << dropSensor->GetPosition() << "," << ds->GetBatteryVoltage() << ",";
fout << target->IsValid() << "," << target->IsHot() << "," << target->GetDistance() << "," << target->GetX() << ",";
fout << target->GetY() << "," << target->IsLeft() << "," << target->IsRight() << ",";
fout << ds->GetMatchTime() << "," << auto_state << ",";
fout << endl;
}
}
void RobotInit(void)
{
//Pegs 1-6
dseio.SetDigitalConfig(1,DriverStationEnhancedIO::kInputPullDown);
dseio.SetDigitalConfig(2,DriverStationEnhancedIO::kInputPullDown);
dseio.SetDigitalConfig(3,DriverStationEnhancedIO::kInputPullDown);
dseio.SetDigitalConfig(4,DriverStationEnhancedIO::kInputPullDown);
dseio.SetDigitalConfig(5,DriverStationEnhancedIO::kInputPullDown);
dseio.SetDigitalConfig(6,DriverStationEnhancedIO::kInputPullDown);
//pickup,retrieve,stow,stop
dseio.SetDigitalConfig(7,DriverStationEnhancedIO::kInputPullDown);
dseio.SetDigitalConfig(8,DriverStationEnhancedIO::kInputPullDown);
dseio.SetDigitalConfig(9,DriverStationEnhancedIO::kInputPullDown);
dseio.SetDigitalConfig(10,DriverStationEnhancedIO::kInputPullDown);
LP1=false;
LP2 =false;
LP3=false;
LP4 =false;
LP5=false;
LP6=false;
LPStow=false;
LPRetrieve=false;
LPPickUp=false;
LPStopArm=true;
myarm->MoveClaw(-1);
//start compressor
robot_compressor->Start();
mygyro->Reset();
}
int main() {
//Handle Ctrl-C quit
signal(SIGINT, sig_handler);
Shield *shield = new Shield();
//two motor setup
Motor left_motor(15, 0);
Motor right_motor(4, 2);
Gyro gyro;
IR medA = IR(2, 6149.816568, 4.468768853);
IR medB = IR(1, 2391.189039, -0.079559138);
//was 0.015, 0, 0.4
//also 0.05, 0, 0.2
//for medA:
PIDDrive driveA(&left_motor, &right_motor, shield, 0.00001, 0.0001, 0.2);
PIDDrive driveB(&left_motor, &right_motor, shield, 0.00001, 0.0001, 0.1);
int straight = 0;
int turning = 0;
double curAngle = gyro.get_angle();
/*double startDist = medA.getDistance();
double dist[5] = {startDist, startDist, startDist, startDist, startDist};
double avg = startDist;
double sum = 0;
*/
while (running) {
/*sum = 0;
for (int i = 1; i <= 4; i++) {
dist[i-1] = dist[i];
}
dist[4] = medA.getDistance();
for (int i = 0; i < 5; i++) {
sum += dist[i];
}
avg = sum / 5.;
*/
std::cout << "sensor A: " << medA.getDistance() << "\t";
std::cout << "sensor B: " << medB.getDistance() << std::endl;
//std::cout << "avg: " << avg << std::endl;
/*if (medA.getDistance() < 20 && medB.getDistance() > 30) {
turning = 0;
if (straight == 0) {
curAngle = gyro.get_angle();
}
straight++;
driv.drive(curAngle, gyro.get_angle(), 0.25); //keep driving straight
std::cout << "straight\t" << "angle: " << curAngle << std::endl;
} else if ((medA.getDistance() < 20 && medB.getDistance() < 30) || (medA.getDistance() > 30)) {
straight = 0;
if (turning == 0) {
curAngle = gyro.get_angle();
}
turning++;
//drive.drive(curAngle + 10, gyro.get_angle(), .2); //turn away from wall
drive.stop();
straight = 0;
sleep(.2);
std::cout << "turning " << "angle: " << curAngle << std::endl;
} else {
drive.drive(gyro.get_angle(), gyro.get_angle(), 0.2);
}*/
if (medB.getDistance() < 15) {
driveA.stop();
driveB.stop();
while (medB.getDistance() < 30) {
left_motor.setSpeed(shield, 0.2);
right_motor.setSpeed(shield, -0.2);
std::cout << "B" << std::endl;
}
} else if (medA.getDistance() > 80) {
left_motor.setSpeed(shield, 0.2);
right_motor.setSpeed(shield, -0.2);
usleep(300000);
} else {
turning = 0;
driveA.drive(15, medA.getDistance(), 0.2);
std::cout << "A" << std::endl;
usleep(100000);
}
/*
if (medA.getDistance() < 14) {
left_motor.setSpeed(shield, 0.2);
right_motor.setSpeed(shield, -0.15);
} else if (medA.getDistance() > 16) {
left_motor.setSpeed(shield, -0.15);
right_motor.setSpeed(shield, 0.2);
} else {
left_motor.setSpeed(shield, 0.2);
right_motor.setSpeed(shield, 0.2);
}
*/
/*if (avg < 30) {
drive.drive(gyro.get_angle(), gyro.get_angle(), -.25);
} else {
drive.drive(gyro.get_angle(), gyro.get_angle(), 0.25);
}*/
usleep(50000);
//sleep(1);
}
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()//teleop code
{
robotDrive.SetSafetyEnabled(false);
gyro.Reset();
grabEncoder.Reset();
timer.Start();
timer.Reset();
double liftHeight = 0; //variable for lifting thread
int liftHeightBoxes = 0; //another variable for lifting thread
int liftStep = 0; //height of step in inches
int liftRamp = 0; //height of ramp in inches
double grabPower;
bool backOut;
uint8_t toSend[10];//array of bytes to send over I2C
uint8_t toReceive[10];//array of bytes to receive over I2C
uint8_t numToSend = 1;//number of bytes to send
uint8_t numToReceive = 0;//number of bytes to receive
toSend[0] = 1;//set the byte to send to 1
i2c.Transaction(toSend, 1, toReceive, 0);//send over I2C
bool isGrabbing = false;//whether or not grabbing thread is running
bool isLifting = false;//whether or not lifting thread is running
bool isBraking = false;//whether or not braking thread is running
float driveX = 0;
float driveY = 0;
float driveZ = 0;
float driveGyro = 0;
bool liftLastState = false;
bool liftState = false; //button pressed
double liftLastTime = 0;
double liftTime = 0;
bool liftRan = true;
Timer switchTimer;
Timer grabTimer;
switchTimer.Start();
grabTimer.Start();
while (IsOperatorControl() && IsEnabled())
{
// Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
// This sample does not use field-oriented drive, so the gyro input is set to zero.
toSend[0] = 1;
numToSend = 1;
driveX = driveStick.GetRawAxis(Constants::driveXAxis);//starts driving code
driveY = driveStick.GetRawAxis(Constants::driveYAxis);
driveZ = driveStick.GetRawAxis(Constants::driveZAxis);
driveGyro = gyro.GetAngle() + Constants::driveGyroTeleopOffset;
if (driveStick.GetRawButton(Constants::driveOneAxisButton)) {//if X is greater than Y and Z, then it will only go in the direction of X
toSend[0] = 6;
numToSend = 1;
if (fabs(driveX) > fabs(driveY) && fabs(driveX) > fabs(driveZ)) {
driveY = 0;
driveZ = 0;
}
else if (fabs(driveY) > fabs(driveX) && fabs(driveY) > fabs(driveZ)) {//if Y is greater than X and Z, then it will only go in the direction of Y
driveX = 0;
driveZ = 0;
}
else {//if Z is greater than X and Y, then it will only go in the direction of Z
driveX = 0;
driveY = 0;
}
}
if (driveStick.GetRawButton(Constants::driveXYButton)) {//Z lock; only lets X an Y function
toSend[0] = 7;
driveZ = 0;//Stops Z while Z lock is pressed
}
if (!driveStick.GetRawButton(Constants::driveFieldLockButton)) {//robot moves based on the orientation of the field
driveGyro = 0;//gyro stops while field lock is enabled
}
driveX = Constants::scaleJoysticks(driveX, Constants::driveXDeadZone, Constants::driveXMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveXDegree);
driveY = Constants::scaleJoysticks(driveY, Constants::driveYDeadZone, Constants::driveYMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveYDegree);
driveZ = Constants::scaleJoysticks(driveZ, Constants::driveZDeadZone, Constants::driveZMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveZDegree);
robotDrive.MecanumDrive_Cartesian(driveX, driveY, driveZ, driveGyro);//makes the robot drive
if (pdp.GetCurrent(Constants::grabPdpChannel) < Constants::grabManualCurrent) {
pickup.setGrabber(Constants::scaleJoysticks(grabStick.GetX(), Constants::grabDeadZone, Constants::grabMax, Constants::grabDegree)); //defines the grabber
if(grabTimer.Get() < 1) {
toSend[0] = 6;
}
}
else {
pickup.setGrabber(0);
grabTimer.Reset();
toSend[0] = 6;
}
if (Constants::grabLiftInverted) {
pickup.setLifter(-Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree)); //defines the lifter
}
else {
pickup.setLifter(Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree)); //defines the lifter
}
SmartDashboard::PutNumber("Lift Power", Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree));
SmartDashboard::PutBoolean("Is Lifting", isLifting);
if (Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree) != 0 || isLifting) { //if the robot is lifting
isBraking = false; //stop braking thread
SmartDashboard::PutBoolean("Braking", false);
}
else if(!isBraking) {
isBraking = true; //run braking thread
pickup.lifterBrake(isBraking);//brake the pickup
}
if (grabStick.GetRawButton(Constants::liftFloorButton)) {
liftHeight = 0;
pickup.lifterPosition(liftHeight, isLifting, grabStick);//start lifting thread
liftRan = true;
}
liftTime = timer.Get();
liftState = grabStick.GetRawButton(Constants::liftButton);
if (liftState) { //if button is pressed
if (!liftLastState) {
if (liftTime - liftLastTime < Constants::liftMaxTime) {
if (liftHeightBoxes < Constants::liftMaxHeightBoxes) {
liftHeightBoxes++; //adds 1 to liftHeightBoxes
}
}
else {
liftHeightBoxes = 1;
liftRamp = 0;
liftStep = 0;
}
}
liftLastTime = liftTime;
liftLastState = true;
liftRan = false;
}
else if (grabStick.GetRawButton(Constants::liftRampButton)) {
if (liftTime - liftLastTime > Constants::liftMaxTime) {
liftHeight = 0;
liftStep = 0;
}
liftRamp = 1; //prepares to go up ramp
liftLastTime = liftTime;
liftRan = false;
}
else if (grabStick.GetRawButton(Constants::liftStepButton)) {
if (liftTime - liftLastTime > Constants::liftMaxTime) {
liftHeight = 0;
liftRamp = 0;
}
liftStep = 1; //prepares robot for step
liftLastTime = liftTime;
liftRan = false;
}
else {
if (liftTime - liftLastTime > Constants::liftMaxTime && !liftRan) {
liftHeight = liftHeightBoxes * Constants::liftBoxHeight + liftRamp * Constants::liftRampHeight + liftStep * Constants::liftStepHeight; //sets liftHeight
if (liftHeightBoxes > 0) {
liftHeight -= Constants::liftBoxLip;
}
pickup.lifterPosition(liftHeight, isLifting, grabStick);//start lifting thread
liftRan = true;
}
liftLastState = false;
}
if (grabStick.GetRawButton(Constants::grabToteButton)) {//if grab button is pressed
grabPower = Constants::grabToteCurrent;
backOut = true;
if (!isGrabbing) {
pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);//start grabber thread
}
}
else if (grabStick.GetRawButton(Constants::grabBinButton)) {//if grab button is pressed
grabPower = Constants::grabBinCurrent;
backOut = false;
if (!isGrabbing) {
pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);//start grabber thread
}
}
else if (grabStick.GetRawButton(Constants::grabChuteButton)) {//if grab button is presset
SmartDashboard::PutBoolean("Breakpoint -2", false);
SmartDashboard::PutBoolean("Breakpoint -1", false);
SmartDashboard::PutBoolean("Breakpoint 0", false);
SmartDashboard::PutBoolean("Breakpoint 1", false);
SmartDashboard::PutBoolean("Breakpoint 2", false);
SmartDashboard::PutBoolean("Breakpoint 3", false);
SmartDashboard::PutBoolean("Breakpoint 4", false);
//Wait(.5);
if (!isGrabbing) {
//pickup.grabberChute(isGrabbing, grabStick);//start grabber thread
}
}
//determines what the LED's look like based on what the Robot is doing
if (isGrabbing) {
toSend[0] = 5;
numToSend = 1;
}
if (isLifting) {//if the grabbing thread is running
if (Constants::encoderToDistance(liftEncoder.Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) < liftHeight) {
toSend[0] = 3;
}
else {
toSend[0] = 4;
}
numToSend = 1;//sends 1 byte to I2C
}
if(!grabOuterLimit.Get()) { //tells if outer limit is hit with lights
if(switchTimer.Get() < 1) {
toSend[0] = 6;
}
}
else {
switchTimer.Reset();
}
if (driveStick.GetRawButton(Constants::sneakyMoveButton)) {
toSend[0] = 0;
numToSend = 1;
}
float distance = prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12; // distance from ultrasonic sensor
float rotations = (float) liftEncoder.Get(); // rotations on encoder
SmartDashboard::PutNumber("Distance", distance); // write stuff to smart dash
SmartDashboard::PutNumber("Current", pdp.GetCurrent(Constants::grabPdpChannel));
SmartDashboard::PutNumber("LED Current", pdp.GetCurrent(Constants::ledPdpChannel));
SmartDashboard::PutNumber("Lift Encoder", rotations);
SmartDashboard::PutNumber("Lift Height", liftHeight);
SmartDashboard::PutNumber("Grab Encoder", grabEncoder.Get());
SmartDashboard::PutBoolean("Grab Inner", grabInnerLimit.Get());
SmartDashboard::PutBoolean("Grab Outer", grabOuterLimit.Get());
SmartDashboard::PutNumber("Drive Front Left Current", pdp.GetCurrent(Constants::driveFrontLeftPin));
SmartDashboard::PutNumber("Drive Front Right Current", pdp.GetCurrent(Constants::driveFrontRightPin));
SmartDashboard::PutNumber("Drive Rear Left Current", pdp.GetCurrent(Constants::driveRearLeftPin));
SmartDashboard::PutNumber("Drive Rear Right Current", pdp.GetCurrent(Constants::driveRearRightPin));
SmartDashboard::PutNumber("Throttle", grabStick.GetZ());
i2c.Transaction(toSend, 1, toReceive, 0);//send and receive information from arduino over I2C
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
} //end of teleop
isBraking = false;
toSend[0] = 0;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
}
/**
* 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 RA14Robot::AutonomousPeriodic() {
StartOfCycleMaintenance();
target->Parse(server->GetLatestPacket());
float speed = Config::GetSetting("auto_speed", .5);
cout<<"Auto Speed: "<<Config::GetSetting("auto_speed", 0)<<endl; // original .1
float angle = gyro->GetAngle();
float error = targetHeading - angle;
float corrected = error * Config::GetSetting("auto_heading_p", .01);
//float corrected = error * Config::GetSetting("auto_heading_p", .01);
cout <<"Gyro angle: "<<angle<<endl;
cout <<"Error: " << error << endl;
//float lDrive = Config::GetSetting("auto_speed", -0.3) + (error * Config::GetSetting("auto_heading_p", .01));
//float rDrive = Config::GetSetting("auto_speed", -0.3) - (error * Config::GetSetting("auto_heading_p", .01));
// Reading p value from the config file does not appear to be working. When we get p from config, the math is not correct.
float lDrive = Config::GetSetting("auto_speed", -0.3) + (error*0.01);
float rDrive = Config::GetSetting("auto_speed", -0.3) - (error*0.01);
cout << "Left: " << lDrive << endl;
cout << "Right: " << rDrive << endl;
#ifndef DISABLE_AUTONOMOUS
switch(auto_case)
{
case 0:
// start master autonomous mode
switch (auto_state) {
case 0: // start
auto_timer->Reset();
auto_timer->Start();
myCam->Process(false, false, false);
break;
case 1:
myCam->Process(false, false, false);
if (target->IsValid()) {
auto_state = 2;
} else if (auto_timer->Get() >= Config::GetSetting("auto_target_timeout", 1)) {
auto_state = 10;
}
break;
case 2:
myCam->Process(false, false, false);
if (target->IsHot()) {
auto_state = 10;
} else {
if (auto_timer->Get() >= Config::GetSetting("auto_target_hot_timeout", 5)) {
auto_state = 10;
}
}
break;
case 10:
myCam->Process(true, false, false);
if (myCam->IsReadyToRearm()) {
auto_state = 11;
}
break;
case 11:
myCam->Process(false, false, false);
myDrive->DriveArcade(corrected, speed);
if (myDrive->GetOdometer() >= Config::GetSetting("auto_drive_distance", 100))
{
myDrive->Drive(0,0);
}
break;
case 12:
myDrive->Drive(0,0);
break;
default:
cout << "Unknown state #" << auto_state << endl;
break;
}
// end master autonomous mode
break;
case 1:
if( target->IsHot() && target->IsValid() )
{
cout << "Target is HOTTT taking the shot" << endl;
//Drive forward and shoot right away
//if( target->IsLeft() || target->IsRight() )
//{
if(myDrive->GetOdometer() <= 216 - Config::GetSetting("auto_firing_distance", 96)) //216 is distance from robot to goal
{
myDrive->Drive(corrected,speed);
}
else
{
myDrive->Drive(0,0);
cout << "FIRING" << endl;
#ifndef DISABLE_SHOOTER
myCam->Process(1,0,0);
#endif //Ends DISABLE_SHOOTER
}
/*}
else
{
cout<<"Error"<<endl;
}*/
}
else if(target->IsValid())
{
cout << "Target is valid, but cold. Driving and waiting" << endl;
//Drive forward and wait to shoot
if(myDrive->GetOdometer() <= 216 - Config::GetSetting("auto_firing_distance", 96)) //216 is distance from robot to goal
{
myDrive->Drive(corrected, speed);
}
else
{
// now at the firing spot.
myDrive->Drive(0,0);
if( target->IsHot() )
{
cout << "FIRING" << endl;
#ifndef DISABLE_SHOOTER
myCam->Process(1,0,0);
#endif //Ends DISABLE_SHOOTER
}
}
}
else
{
//Not valid
cout << "Not valid target." << endl;
}
break;
case 2:
#ifndef DISABLE_SHOOTER
myCam->Process(false,false,false);
if(auto_timer->Get() >= 4.0)
{
myCam->Process(true,false,false);
}
#endif //Ends DISABLE_SHOOTER
/*if(myDrive->GetOdometer() <= 216 - Config::GetSetting("auto_firing_distance", 96)) //216 is distance from robot to goal
{
cout<<"Distance traveled: "<<myDrive->GetOdometer()<<" inches"<<endl;
myDrive->Drive(corrected, speed);
}
else
{
myDrive->Drive(0,0);
cout << "FIRING" << endl;
}*/
if(auto_timer->Get() < 5.0)
{
cout<<"Waiting....."<<endl;
}
else
{
cout<<"Distance traveled: "<<myDrive->GetOdometer()<<" inches"<<endl;
myDrive->Drive(-.5, -.5);
}
break;
case 3:
#ifndef DISABLE_SHOOTER
switch(auto_state) {
case 0:
// Home/rearm
// fire, rearm, eject
myCam->Process(false, false, false);
if (myCam->IsReadyToFire()) {
auto_state = 1;
}
break;
case 1:
// fire
myCam->Process(true, false, false);
if (myCam->IsReadyToRearm()) {
auto_state = 2;
}
break;
case 2:
// rearm
myCam->Process(false, true, false);
if (myCam->IsReadyToFire()) {
auto_state = 3;
auto_timer->Reset();
auto_timer->Start();
}
break;
case 3:
myCam->Process(false, false, false);
myCollection->SpinMotor(Config::GetSetting("intake_roller_speed", .7));
if (auto_timer->HasPeriodPassed( Config::GetSetting("auto_collection_delay", 1.0) )) {
auto_state = 4;
}
break;
case 4:
// fire again!
myCam->Process(true, false, false);
auto_state = 5;
break;
case 5:
myCam->Process(false, false, false);
if(myDrive->GetOdometer() <= 216 - Config::GetSetting("auto_firing_distance", 96)) //216 is distance from robot to goal
{
myDrive->Drive(corrected, speed);
} else {
// now at the firing spot.
auto_state = 6;
myDrive->Drive(0,0);
}
break;
case 6:
myCollection->RetractArm();
break;
default:
cout << "Error, unrecognized state " << auto_state << endl;
}
#endif //Ends DISABLE_SHOOTER
break;
case 4: /* One ball Autonomous - Drive forward specific distance, stop then shoot.*/
#ifndef DISABLE_SHOOTER
switch(auto_state) {
case 0: //Reset odometer, lower the arm and set launcher to ready to fire
myDrive->ShiftUp();
myDrive->ResetOdometer();
myCollection->ExtendArm();
myCam->Process(false,true,false);
auto_state = 1;
break;
case 1: // Start driving forward
// myDrive->Drive(-.5, -.5);
//myDrive->Drive(lDrive, rDrive);
myDrive->DriveArcade(corrected, speed);
auto_state = 2;
break;
case 2: // Continue driving until required distance
if(myDrive->GetOdometer() >= Config::GetSetting("auto_drive_distance", 96)) {
myDrive->Drive(0, 0);
auto_state = 3;
}
break;
case 3: // Fire launcher
myCam->Process(true,false,false);
auto_state = 4;
break;
case 4: // Idle state
break;
}
#endif //Ends DISABLE_SHOOTER
break;
case 5: // Two Ball Autonomous - Drag ball 2 while driving forward specific distance, stop then shoot, load shoot next ball
#ifndef DISABLE_SHOOTER
// By Hugh Meyer - April 1, 2014
switch(auto_state) {
cout << "Executing mr m's auton" << endl;
case 0: // Set low gear, reset odometer, extend pickup arm, set launcher ready to fire, set wait timer
//myDrive->ShiftUp(); // Shift to low gear
myDrive->ShiftDown();
myDrive->ResetOdometer(); // Reset odometer to zero
myCollection->ExtendArm(); // Extend arm to pickup position
myCam->Process(false,true,false); // Set launcher to ready to fire position
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while pickup extends
auto_state = 1; // Go on to next state
break;
case 1: // Wait for timer to expire - Let arm get extended and stabalized
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton5_extend_delay", 1.0) )) {
auto_state = 2;
}
break;
case 21: //reset gyro and reset timer
gyro->Reset();
auto_timer->Reset();
auto_timer->Start();
auto_state = 22;
break;
case 22:
if(auto_timer->HasPeriodPassed(Config::GetSetting("auton5_gyro_reset_delay", 2) )){
auto_state = 2;
}
break;
case 2: // Activate pickup roller motor to drag speed, start driving forward
myCollection->SpinMotor(Config::GetSetting("auton5_drag_speed", 0.3)); // Start motor to drag ball 2
//myDrive->DriveArcade(corrected, speed); // Drive straight
myDrive->DriveArcade(0.0, Config::GetSetting("auton5_drive_speed", -0.7)); //Start driving forwards
auto_state = 3;
break;
case 3: // Continue driving until required distance, stop driving, stop pickup roller motor
if(myDrive->GetOdometer() >= Config::GetSetting("auton5_drive_distance", 96.0))
{
myCollection->SpinMotor(0); // Stop collector pickup motor
myDrive->Drive(0, 0); // Stop driving
auto_state = 31; // On to next state
}
break;
case 31: // slightly un-eject herded ball to avoid contact with launch ball
myCollection->SpinMotor(Config::GetSetting("auton5_eject_speed", 0.3));
auto_timer->Reset();
auto_timer->Start(); // setup timer to time un-eject
auto_state = 32;
break;
case 32: // once timer has run out, stop ejection and fire
if (auto_timer->HasPeriodPassed(Config::GetSetting("auton5_uneject_time", 0.25))) {
myCollection->SpinMotor(0); // stop spinner
auto_state = 4;
}
break;
case 4: // Fire launcher to shoot first ball, set wait timer
myCam->Process(true,false,false); // Fire ball # 1
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball launches
auto_state = 5; // On to next state
break;
case 5: // Wait for timer to expire after ball one launches
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton5_ball_1_launch_delay", 1.0) )) {
auto_state = 6; // On to next state
}
break;
case 6: // set launcher ready to fire for ball 2, set wait timer
myCam->Process(false,true,false); // Set launcher to ready to fire position
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball launches
auto_state = 7; // On to next state
break;
case 7: // Wait for timer to expire
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton5_ball_2_ready2fire_delay", 1.0) )) {
auto_state = 8; // Wait for launcher to get ready to accept ball 2
}
break;
case 8: // Activate pickup roller motor to load ball 2, set wait timer
myCollection->SpinMotor(Config::GetSetting("auton5_intake_roller_speed", 0.7));
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball 2 loads
auto_state = 9; // On to next state
break;
case 9: // Wait for timer to expire while ball 2 gets collected into launcher
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton5_ball_2_settle_delay", 1.0) )) {
auto_state = 10; // Wait for ball 2 to be collected and settle
}
break;
case 19: // Drive backwards a little bit, set wait timer
myDrive->DriveArcade(-1*corrected, -1*speed); // Drive straight
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball launches
auto_state = 20; // Wait for ball 2 to be collected and settle
break;
case 20: // Wait for timer to expire while ball 2 gets collected into launcher
if (myDrive->GetOdometer() <=
Config::GetSetting("auton5_drive_distance", 96)
- Config::GetSetting("auton5_backup_distance", 6)) {
myDrive->Drive(0,0);
auto_state = 10;
}
break;
case 10: // Fire launcher to shoot second ball and stop roller
myCam->Process(true,false,false); // Fire ball # 2
myCollection->SpinMotor(0); // Stop spinning the roller
auto_state = 11; // All done so go to idle state
break;
case 11: // Idle state
auto_state = 11;
break;
/* case 12: // More states if we need them for changes.
auto_state = 13;
break;
case 13: //
auto_state = 14;
break;
case 14: //
auto_state = 15;
break;
case 15: //
auto_state = 15;
break;
*/
}
#endif //Ends DISABLE_SHOOTER
break;
case 6: // Two Ball Autonomous - Drive forward specific distance, stop then shoot, backup get second ball, drive, shoot
#ifndef DISABLE_SHOOTER
// By Hugh Meyer - April 1, 2014
switch(auto_state) {
case 0: // Set low gear, reset odometer, extend pickup arm, set launcher ready to fire
myDrive->ShiftDown(); // Shift to low gear
myDrive->ResetOdometer(); // Reset odometer to zero
myCollection->ExtendArm(); // Extend arm to pickup position
myCam->Process(false,true,false); // Set launcher to ready to fire position
auto_state = 1; // Go on to next state
break;
case 1: // Drive forward
myDrive->DriveArcade(corrected, speed);
auto_state = 2; // On to next state
break;
case 2: // Continue driving forward until the specific distance is traveled
if(myDrive->GetOdometer() >= Config::GetSetting("auton6_drive_forward_distance", 96.0)) {
myDrive->Drive(0, 0);
auto_state = 3;
}
break;
case 3: // Fire launcher to launch first ball, set timer
myCam->Process(true,false,false); // Launch ball # 1
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball launches
auto_state = 4; // On to next state
break;
case 4: // Wait for timer while ball launcher fires
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton6_ball_1_fire_delay", 1.0) )) {
auto_state = 5; // On to next state
}
break;
case 5: // Reset Odometer, Drive backwards, set launcher to ready to fire position, turn on pickup
cout<<"Corrected Values: "<<corrected<<endl;
cout<<"Speed: "<< -1 * speed<<endl;
myDrive->ResetOdometer(); // Reset odometer to zero
myDrive->DriveArcade(-1* corrected, -1 * speed); // Drive backwards
myCam->Process(false,true,false); // Set launcher to ready to fire position
myCollection->SpinMotor(-1 * Config::GetSetting("auton6_intake_roller_speed", 0.7)); // Turn on pickup
auto_state = 6; // On to next state
break;
case 6: // Continue driving backwards a specific distance
if(myDrive->GetOdometer() <= -1 * Config::GetSetting("auton6_drive_forward_distance", -96.0)) {
auto_state = 7; // On to next state
}
break;
case 7: // Set timer
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball launches
auto_state = 8; // On to next state
break;
case 8: // Wait for timer while ball loads and settles
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton6_ball_2_load_delay", 1.0) )) {
auto_state = 9; // On to next state
myDrive->ResetOdometer();
}
break;
case 9: // Drive forwards
myDrive->DriveArcade(/*corrected*/ 0.01, speed);
auto_state = 10; // On to next state
break;
case 10: // Continue driving forward until the specific distance is traveled
cout << "I am driving forward: " << myDrive->GetOdometer() << endl;
myDrive->DriveArcade(0.01, speed);
if(myDrive->GetOdometer() >= Config::GetSetting("auton6_drive_forward_distance", 96.0)) {
myDrive->Drive(0, 0);
auto_state = 11; // On to next state
}
break;
case 11: // Fire launcher to launch second ball
myCam->Process(true,false,false); // Launch ball # 2
auto_state = 15; // On to next state
break;
/* case 12: //
auto_state = 13; // On to next state
break;
case 13: //
auto_state = 14; // On to next state
break;
case 14: //
auto_state = 15; // All done so go to idle state
break;
*/
case 15: // Idle state
auto_state = 15;
break;
}
cout << myDrive->GetOdometer() << endl;
#endif //Ends DISABLE_SHOOTER
break;
case 7: // Two Ball Autonomous - Drag ball 2 while driving forward specific distance, stop then shoot, load shoot next ball
#ifndef DISABLE_SHOOTER
// By Hugh Meyer - April 1, 2014
switch(auto_state) {
cout << "Executing mr m's auton" << endl;
case 0: // Set low gear, reset odometer, extend pickup arm, set launcher ready to fire, set wait timer
myDrive->ShiftUp(); // Shift to high gear
//myDrive->ShiftDown();
myDrive->ResetOdometer(); // Reset odometer to zero
myCollection->ExtendArm(); // Extend arm to pickup position
myCam->Process(false,true,false); // Set launcher to ready to fire position
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while pickup extends
auto_state = 1; // Go on to next state
break;
case 1: // Wait for timer to expire - Let arm get extended and stabalized
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton7_extend_delay", 1.0) )) {
auto_state = 2;
}
break;
case 21: //reset gyro and reset timer
gyro->Reset();
auto_timer->Reset();
auto_timer->Start();
auto_state = 22;
break;
case 22:
if(auto_timer->HasPeriodPassed(Config::GetSetting("auton7_gyro_reset_delay", 2) )){
auto_state = 2;
}
break;
case 2: // Activate pickup roller motor to drag speed, start driving forward
myCollection->SpinMotor(Config::GetSetting("auton7_drag_speed", 0.3)); // Start motor to drag ball 2
//myDrive->DriveArcade(corrected, speed); // Drive straight
myDrive->DriveArcade(0.05, speed);
auto_state = 3;
break;
case 3: // Continue driving until required distance, stop driving, stop pickup roller motor
if(myDrive->GetOdometer() >= Config::GetSetting("auton7_drive_distance", 96.0))
{
myCollection->SpinMotor(0); // Stop collector pickup motor
myDrive->Drive(0, 0); // Stop driving
auto_state = 31; // On to next state
}
break;
case 31: // slightly un-eject herded ball to avoid contact with launch ball
myCollection->SpinMotor(Config::GetSetting("auton7_eject_speed", 0.3));
auto_timer->Reset();
auto_timer->Start(); // setup timer to time un-eject
auto_state = 32;
break;
case 32: // once timer has run out, stop ejection and fire
if (auto_timer->HasPeriodPassed(Config::GetSetting("auton7_uneject_time", 0.25))) {
myCollection->SpinMotor(0); // stop spinner
auto_state = 4;
}
break;
case 4: // Fire launcher to shoot first ball, set wait timer
myCam->Process(true,false,false); // Fire ball # 1
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball launches
auto_state = 5; // On to next state
break;
case 5: // Wait for timer to expire after ball one launches
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton7_ball_1_launch_delay", 1.0) )) {
auto_state = 6; // On to next state
}
break;
case 6: // set launcher ready to fire for ball 2, set wait timer
myCam->Process(false,true,false); // Set launcher to ready to fire position
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball launches
auto_state = 7; // On to next state
break;
case 7: // Wait for timer to expire
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton7_ball_2_ready2fire_delay", 1.0) )) {
auto_state = 8; // Wait for launcher to get ready to accept ball 2
}
break;
case 8: // Activate pickup roller motor to load ball 2, set wait timer
myCollection->SpinMotor(Config::GetSetting("auton7_intake_roller_speed", 0.7));
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball 2 loads
auto_state = 9; // On to next state
break;
case 9: // Wait for timer to expire while ball 2 gets collected into launcher
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton7_ball_2_settle_delay", 1.0) )) {
auto_state = 10; // Wait for ball 2 to be collected and settle
}
break;
case 19: // Drive backwards a little bit, set wait timer
myDrive->DriveArcade(-1*corrected, -1*speed); // Drive straight
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball launches
auto_state = 20; // Wait for ball 2 to be collected and settle
break;
case 20: // Wait for timer to expire while ball 2 gets collected into launcher
if (myDrive->GetOdometer() <=
Config::GetSetting("auton7_drive_distance", 96)
- Config::GetSetting("auton7_backup_distance", 6)) {
myDrive->Drive(0,0);
auto_state = 10;
}
break;
case 10: // Fire launcher to shoot second ball and stop roller
myCam->Process(true,false,false); // Fire ball # 2
myCollection->SpinMotor(0); // Stop spinning the roller
auto_state = 11; // All done so go to idle state
break;
case 11: // Idle state
auto_state = 11;
break;
/* case 12: // More states if we need them for changes.
auto_state = 13;
break;
case 13: //
auto_state = 14;
break;
case 14: //
auto_state = 15;
break;
case 15: //
auto_state = 15;
break;
*/
}
#endif //Ends DISABLE_SHOOTER
break;
/*
case 7: // Extra structure for more changes
#ifndef DISABLE_SHOOTER
// By Hugh Meyer - April 1, 2014
switch(auto_state) {
case 0: // Set low gear, reset odometer, extend pickup arm, set launcher ready to fire, drive foward
myDrive->ShiftDown(); // Shift to low gear
myDrive->ResetOdometer(); // Reset odometer to zero
myCollection->ExtendArm(); // Extend arm to pickup position
myCam->Process(false,true,false); // Set launcher to ready to fire position
myDrive->Drive(lDrive, rDrive); // Drive straight
auto_state = 1; // Go on to next state
break;
case 1: //
auto_state = 2; // On to next state
break;
case 2: //
auto_state = 3; // On to next state
break;
case 3: //
auto_state = 4; // On to next state
break;
case 4: //
auto_state = 5; // On to next state
break;
case 5: //
auto_state = 6; // On to next state
break;
case 6: //
auto_state = 7; // On to next state
break;
case 7: //
auto_state = 8; // On to next state
break;
case 8: //
auto_state = 9; // On to next state
break;
case 9: //
auto_state = 10; // On to next state
break;
case 10: //
auto_state = 11; // On to next state
break;
case 11: //
auto_state = 12; // On to next state
break;
case 12: //
auto_state = 13; // On to next state
break;
case 13: //
auto_state = 14; // On to next state
break;
case 14: //
auto_state = 15; // All done so go to idle state
break;
case 15: // Idle state
auto_state = 15;
break;
}
#endif //Ends DISABLE_SHOOTER
break;
*/
default:
cout<<"Error in autonomous, unrecognized case: "<<auto_case<<endl;
}
//#else
// if (myDrive->GetOdometer() <= (4 * acos(-1) ) ) //216 is distance from robot to goal
// {
// float speed = Config::GetSetting("auto_speed", .3);
// cout << myDrive->GetOdometer() << endl;
// myDrive->Drive(speed, speed);
// } else {
// cout << "Finished driving";
// myDrive->Drive(0, 0);
// }
#endif //Ends DISABLE_AUTONOMOUS
EndOfCycleMaintenance();
}
void Autonomous()
{
Timer timer;
float power = 0;
bool isLifting = false;
bool isGrabbing = false;
double liftHeight = Constants::liftBoxHeight-Constants::liftBoxLip;
double grabPower = Constants::grabAutoCurrent;
bool backOut;
uint8_t toSend[1];//array of bytes to send over I2C
uint8_t toReceive[0];//array of bytes to receive over I2C
uint8_t numToSend = 1;//number of bytes to send
uint8_t numToReceive = 0;//number of bytes to receive
toSend[0] = 2;//set the byte to send to 1
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);//send over I2C
bool isSettingUp = true;
//pickup.setGrabber(-1); //open grabber all the way
pickup.setLifter(0.8);
while (isSettingUp && IsEnabled() && IsAutonomous()) {
isSettingUp = false;
/*if (grabOuterLimit.Get() == false) {
pickup.setGrabber(0); //open until limit
}
else {
isSettingUp = true;
}*/
if (liftLowerLimit.Get()) {
pickup.setLifter(0); //down till bottom
}
else {
isSettingUp = true;
}
}
gyro.Reset();
liftEncoder.Reset();
grabEncoder.Reset();
if (grabStick.GetZ() > .8) {
timer.Reset();
timer.Start();
while (timer.Get() < 1) {
robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle()); // drive back
if(power>-.4){
power-=0.005;
Wait(.005);
}
}
robotDrive.MecanumDrive_Cartesian(0, 0, 0, gyro.GetAngle()); // STOP!!!
timer.Stop();
timer.Reset();
Wait(1);
}
power = 0;
while (isLifting && IsEnabled() && IsAutonomous()) {
Wait(.005);
}
backOut = Constants::autoBackOut;
pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);
Wait(.005);
while (isGrabbing && IsEnabled() && IsAutonomous()) {
Wait(.005);
}
liftHeight = 3*Constants::liftBoxHeight;
Wait(.005);
pickup.lifterPosition(liftHeight, isLifting, grabStick);
Wait(.005);
while (isLifting && IsEnabled() && IsAutonomous()) {
Wait(.005);
}
while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12 < 2 && IsEnabled() && IsAutonomous()); // while the nearest object is closer than 2 feet
timer.Start();
while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches < Constants::autoBackupDistance && timer.Get() < Constants::autoMaxDriveTime && IsEnabled() && IsAutonomous()) { // while the nearest object is further than 12 feet
if (power < .45) { //ramp up the power slowly
power += .00375;
}
robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle()); // drive back
float distance = prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12; // distance from ultrasonic sensor
SmartDashboard::PutNumber("Distance", distance); // write stuff to smart dash
SmartDashboard::PutNumber("Drive Front Left Current", pdp.GetCurrent(Constants::driveFrontLeftPin));
SmartDashboard::PutNumber("Drive Front Right Current", pdp.GetCurrent(Constants::driveFrontRightPin));
SmartDashboard::PutNumber("Drive Rear Left Current", pdp.GetCurrent(Constants::driveRearLeftPin));
SmartDashboard::PutNumber("Drive Rear Right Current", pdp.GetCurrent(Constants::driveRearRightPin));
SmartDashboard::PutNumber("Gyro Angle", gyro.GetAngle());
SmartDashboard::PutNumber("Distance (in)", prox.GetVoltage() * Constants:: ultrasonicVoltageToInches);
Wait(.005);
}
timer.Reset();
while(timer.Get() < Constants::autoBrakeTime && IsEnabled() && IsAutonomous()) { // while the nearest object is further than 12 feet
robotDrive.MecanumDrive_Cartesian(0,Constants::autoBrakePower,0); ///Brake
}
float turn = 0;
while (fabs(turn) < 85 && IsEnabled() && IsAutonomous()) { //turn 90(ish) degrees
robotDrive.MecanumDrive_Cartesian(0,0,.1);
turn = gyro.GetAngle();
if (turn > 180) {
turn -= 360;
}
}
robotDrive.MecanumDrive_Cartesian(0,0,0); ///STOP!!!
timer.Stop();
toSend[0] = 8;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
while(IsAutonomous() && IsEnabled());
toSend[0] = 0;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
}
/**
* 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()
{
m_robotDrive.MecanumDrive_Cartesian(m_driveStick.GetX(), m_driveStick.GetY(), m_driveStick2.GetX(), m_gyro.GetAngle());
printf("rate: %d\n", (int) m_encoder.GetRaw());
}
void TeleopPeriodic()
{
//camera->GetImage(frame);
//imaqDrawShapeOnImage(frame, frame, { 10, 10, 100, 100 }, DrawMode::IMAQ_DRAW_VALUE, ShapeMode::IMAQ_SHAPE_OVAL, 0.0f);
//CameraServer::GetInstance()->SetImage(frame);
printf("Left Encoder: %i, Right Encoder: %i, Gyro: %f\n", leftEnc->Get(), rightEnc->Get(), gyro->GetAngle());
drive->ArcadeDrive(driveStick);
drive->SetMaxOutput((1-driveStick->GetThrottle())/2);
//printf("%f\n", (1-stick->GetThrottle())/2);
//leftMotor->Set(0.1);
//rightMotor->Set(0.1);
if (shootStick->GetRawAxis(3) > 0.5) {
launch1->Set(1.0);
launch2->Set(1.0);
} else if (shootStick->GetRawAxis(2) > 0.5) {
printf("Power Counter: %i\n", powerCounter);
if (powerCounter < POWER_MAX) {
powerCounter++;
launch1->Set(-0.8);
launch2->Set(-0.8);
} else {
launch1->Set(-0.6);
launch2->Set(-0.6);
}
} else {
launch1->Set(0.0);
launch2->Set(0.0);
powerCounter = 0.0;
}
//use this button to spin only one winch, to lift up.
if (shootStick->GetRawButton(7)) {
otherWinch->Set(0.5);
} else if (shootStick->GetRawButton(8)) {
otherWinch->Set(-0.5);
} else {
otherWinch->Set(0.0);
}
if (shootStick->GetRawButton(5)) {
winch->Set(-0.7);
if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
// otherWinch->Set(-0.5);
}
} else if (shootStick->GetRawButton(6)) {
winch->Set(0.7);
if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
// otherWinch->Set(0.5);
}
} else {
winch->Set(0.0);
if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
//, otherWinch->Set(0.0);
}
}
if (shootStick->GetRawButton(1)) {
launchPiston->Set(1);
} else {
launchPiston->Set(0);
}
if (shootStick->GetRawButton(3)) {
Autonomous::alignWithGoal(drive, launch1, launch2, winch, otherWinch, table, timer);
}
if (shootStick->GetRawButton(3) && debounce == false) {
debounce = true;
if (defenseUp) {
defensePiston->Set(DoubleSolenoid::Value::kReverse);
defenseUp = false;
} else {
defenseUp =true;
defensePiston->Set(DoubleSolenoid::Value::kForward);
}
} else if (!shootStick->GetRawButton(3)){
debounce = false;
}
}
/****************************************
* Runs the motors with arcade steering.*
****************************************/
void OperatorControl(void)
{
//TODO put in servo for lower camera--look in WPI to set
// Watchdog baddog;
// baddog.Feed();
myRobot.SetSafetyEnabled(true);
//SL Earth.Start(); // turns on Earth
// SmartDashboard *smarty = SmartDashboard::GetInstance();
//DriverStationLCD *dslcd = DriverStationLCD::GetInstance(); // don't press SHIFT 5 times; this line starts up driver station messages (in theory)
//char debugout [100];
compressor.Start();
gyro.Reset(); // resets gyro angle
int rpmForShooter;
while (IsOperatorControl()) // while is the while loop for stuff; this while loop is for "while it is in Teleop"
{
// baddog.Feed();
//myRobot.SetSafetyEnabled(true);
//myRobot.SetExpiration(0.1);
float leftYaxis = driver.GetY();
float rightYaxis = driver.GetTwist();//RawAxis(5);
myRobot.TankDrive(leftYaxis,rightYaxis); // drive with arcade style (use right stick)for joystick 1
float random = gamecomponent.GetY();
float lazysusan = gamecomponent.GetZ();
//bool elevator = Frodo.Get();
float angle = gyro.GetAngle();
bool balance = Smeagol.Get();
SmartDashboard::PutNumber("Gyro Value",angle);
int NumFail = -1;
//bool light = Pippin.Get();
//SL float speed = Earth.GetRate();
//float number = shooter.Get();
//bool highspeed = button1.Get()
//bool mediumspeed = button2.Get();
//bool slowspeed = button3.Get();
bool finder = autotarget.Get();
//bool targetandspin = autodistanceandspin.Get();
SmartDashboard::PutString("Targeting Activation","");
//dslcd->Clear();
//sprintf(debugout,"Number=%f",angle);
//dslcd->Printf(DriverStationLCD::kUser_Line2,2,debugout);
//SL sprintf(debugout,"Number=%f",speed);
//SL dslcd->Printf(DriverStationLCD::kUser_Line4,4,debugout);
//sprintf(debugout,"Number=%f",number);
//dslcd->Printf(DriverStationLCD::kUser_Line1,1,debugout);
//sprintf(debugout,"Finder=%u",finder);
//dslcd->Printf(DriverStationLCD::kUser_Line5,5,debugout);
//dslcd->UpdateLCD(); // update the Driver Station with the information in the code
// sprintf(debugout,"Number=%u",maxi);
// dslcd->Printf(DriverstationLCD::kUser_Line6,5,debugout)
bool basketballpusher = julesverne.Get();
bool bridgetipper = joystickbutton.Get();
if (bridgetipper) // if joystick button 7 is pressed (is true)
{
solenoid.Set(true); // then the first solenoid is on
}
else
{
//Wait(0.5); // and then the first solenoid waits for 0.5 seconds
solenoid.Set(false); //and then the first solenoid turns off
}
if (basketballpusher) // if joystick button 6 is pressed (is true)
{
shepard.Set(true); // then shepard is on the run
//Wait(0.5); // and shepard waits for 0.5 seconds
}
else
{
shepard.Set(false); // and then shepard turns off
} //10.19.67.9 IP address of computer;255.0.0.0 subnet mask ALL FOR WIRELESS CONNECTION #2
//}
//cheetah.Set(0.3*lazysusan);
// smarty->PutDouble("pre-elevator",lynx.Get());
lynx.Set(random);
// smarty->PutDouble("elevator",lynx.Get());
// smarty->PutDouble("joystick elevator",random);
if (balance) // this is the start of the balancing code
{
angle = gyro.GetAngle();
myRobot.Drive(-0.03*angle, 0.0);
Wait(0.005);
}
/*if (light) //button 5 turns light on oand off on game controller
flashring.Set(Relay::kForward);
else
flashring.Set(Relay::kOff);
*/
if (finder)
{
flashring.Set(Relay::kForward);
if (button_H.Get()==true)
{
targeting.SetLMHTarget(BOGEY_H);
SmartDashboard::PutString("Targeting","High Button Pressed");
}
if (button_M.Get()==true)
{
targeting.SetLMHTarget(BOGEY_M);
SmartDashboard::PutString("Targeting","Medium Button Pressed");
}
if (button_L.Get()==true)
{
targeting.SetLMHTarget(BOGEY_L);
SmartDashboard::PutString("Targeting","Low Button Pressed");
}
if (button_H.Get()==true || button_M.Get()==true || button_L.Get()==true)
{
if (targeting.ProcessOneImage())
{
NumFail = 0;
SmartDashboard::PutString("Targeting Activation","YES");
targeting.ChooseBogey();
targeting.MoveTurret();
#ifdef USE_HARDWIRED_RPM
shooter.setTargetRPM(HARDWIRED_RPM);
#else
rpmForShooter = targeting.GetCalculatedRPM();
shooter.setTargetRPM(rpmForShooter);
#endif
targeting.InteractivePIDSetup();
}
else
{
NumFail++;
if (NumFail > 10)
targeting.StopPID();
}
SmartDashboard::PutNumber("Numfail", NumFail);
shooter.setTargetRPM(rpmForShooter);
}
else
{
SmartDashboard::PutString("Targeting Activation","NO");
shooter.setTargetRPM(0);
targeting.StopPID();
}
}
else
{
flashring.Set(Relay::kOff);
targeting.StopPID();
turret.Set(lazysusan); // the lazy susan would turn right & left based on how far the person moves the right joystick#2 side to side
//targeting.StopPID();
//if (elevator) //shooter would move at full speed if button is pressed
//TODO Change RPM values
//TODO Disable calculation of RPM values
SmartDashboard::PutNumber("CurrentRPM",shooter.GetCurrentRPM());
if (button_H.Get() == true)
shooter.setTargetRPM((int)2100);
//From front of free throw line, should hit the backboard and go in
//used to be 2700 RPMs
else if (button_M.Get() == true)
shooter.setTargetRPM((int)1900);
//From front of free throw line, should go in the net--can shoot the next ball on the overshoot?
//Used to be 2250 RPMs
else if (button_L.Get() == true)
shooter.setTargetRPM((int)1350);
//From fender, should hit the backboard
//Used to be 2000 RPMs
//shooter.Set(0.5);
else
shooter.setTargetRPM(0);
// else if (mediumspeed)
//shooter.setTargetRPM((int)0);
//else if (slowspeed)
//shooter.setTargetRPM((int)0);
/*if (targetandspin) //code for autotargeting and speed will go here
{
shooter.setTargetRPM((int)1800);
}
else
{*/
//}
myRobot.TankDrive(leftYaxis,rightYaxis);
}
//Wait(0.005);
}
}
void AutonomousPeriodic()
{
drive->SetMaxOutput(1.0);
printf("Distance: %f\n", rightEnc->GetDistance());
// if (!launcherDown) {
// if (timer->Get() > 1.0) {
// winch->Set(0.5);
// otherWinch->Set(0.5);
// } else if (timer->Get() < 3.0) {
// winch->Set(0.5);
// otherWinch->Set(0.0);
// } else {
// winch->Set(0.0);
// otherWinch->Set(0.0);
// launcherDown = true;
// }
// }
if(done) {
winch->Set(0.0);
otherWinch->Set(0.0);
drive->ArcadeDrive(0.0,0.0);
if (autoCounter > 10) {
launchPiston->Set(0);
launch1->Set(0.0);
launch2->Set(0.0);
} else {
autoCounter++;
if (shoot == "Yes") {
launch1->Set(1.0);
launch2->Set(1.0);
}
}
} else {
if (autoSelected == "Approach Only") {
done = Autonomous::approachOnly();
launch1->Set(0.0);
launch2->Set(0.0);
winch->Set(0.0);
otherWinch->Set(0.0);
} else if (!defenseCrossed) {
if(Autonomous::crossFunctions.find(autoSelected) != Autonomous::crossFunctions.end()) {
bool (*crossFunction)() = Autonomous::crossFunctions.at(autoSelected);
defenseCrossed = crossFunction();
launch1->Set(0.0);
launch2->Set(0.0);
winch->Set(0.0);
otherWinch->Set(0.0);
} else {
done = true;
launch1->Set(0.0);
launch2->Set(0.0);
winch->Set(0.0);
otherWinch->Set(0.0);
drive->ArcadeDrive(0.0,0.0);
}
timer->Reset();
} else {
if (autoSelected == "Spy Bot") {
rotation = 90;
}
//after we cross...
float difference = gyro->GetAngle() - rotation;
if (difference > 10) {
launch1->Set(0.0);
launch2->Set(0.0);
winch->Set(0.0);
otherWinch->Set(0.0);
drive->ArcadeDrive(0.0,difference * 0.3);
timer->Reset();
} else {
if (goal == "Yes") {
if (!Autonomous::alignWithGoal(drive, launch1, launch2, winch, otherWinch, table, timer)) {
launchPiston->Set(0);
} else {
launch1->Set(0.0);
launch2->Set(0.0);
winch->Set(0.0);
otherWinch->Set(0.0);
drive->ArcadeDrive(0.0,0.0);
launchPiston->Set(1);
done = true;
}
} else {
launch1->Set(0.0);
launch2->Set(0.0);
winch->Set(0.0);
otherWinch->Set(0.0);
done = true;
}
}
}
}
}
int main() {
//Handle Ctrl-C quit
signal(SIGINT, sig_handler);
Shield *shield = new Shield();
//two motor setup
Motor left_motor(15, 2);
Motor right_motor(4, 8);
Gyro gyro;
IR medA = IR(1, 6149.816568, 4.468768853);
//was 0.015, 0, 0.4
PIDDrive drive(&left_motor, &right_motor, shield, 0.05, 0, 0.2);
int straight = 0;
int turning = 0;
double curAngle = gyro.get_angle();
/*double startDist = medA.getDistance();
double dist[5] = {startDist, startDist, startDist, startDist, startDist};
double avg = startDist;
double sum = 0;
*/
while (running) {
/*sum = 0;
for (int i = 1; i <= 4; i++) {
dist[i-1] = dist[i];
}
dist[4] = medA.getDistance();
for (int i = 0; i < 5; i++) {
sum += dist[i];
}
avg = sum / 5.;
*/
std::cout << "sensor: " << medA.getDistance() << std::endl;
//std::cout << "avg: " << avg << std::endl;
if (medA.getDistance() < 30) {
if (turning == 0) {
curAngle = gyro.get_angle();
}
turning++;
drive.drive(curAngle + 90, gyro.get_angle(), .25); //turn away from wall
straight = 0;
sleep(.75);
std::cout << "turning " << "angle: " << curAngle << std::endl;
} else {
turning = 0;
if (straight == 0) {
curAngle = gyro.get_angle();
}
straight++;
drive.drive(curAngle, gyro.get_angle(), 0.25); //keep driving straight
std::cout << "straight\t" << "angle: " << curAngle << std::endl;
//sleep(0.25);
}
/*if (avg < 30) {
drive.drive(gyro.get_angle(), gyro.get_angle(), -.25);
} else {
drive.drive(gyro.get_angle(), gyro.get_angle(), 0.25);
}*/
usleep(30000);
}
}
/**
* Reset the gyro.
* Resets the gyro to a heading of zero. This can be used if there is significant
* drift in the gyro and it needs to be recalibrated after it has been running.
* @param slot The slot the analog module is connected to
* @param channel The analog channel the gyro is plugged into
*/
void ResetGyro(UINT32 slot, UINT32 channel)
{
Gyro *gyro = AllocateGyro(slot, channel);
if (gyro) gyro->Reset();
}
static void SetUpTestCase() {
// The gyro object blocks for 5 seconds in the constructor, so only
// construct it once for the whole test case
m_gyro = new Gyro(TestBench::kCameraGyroChannel);
m_gyro->SetSensitivity(0.013);
}
void Autonomous()
{
GetWatchdog().SetEnabled(false);
Timer* hotGoalTimer = new Timer();
Timer* reloadTimer = new Timer();
Timer* intakeTimer = new Timer();
Timer* intakeDropTimer = new Timer();
bool goalFound = false;
//bool rightSideHot = false;
hotGoalTimer->Reset();
hotGoalTimer->Start();
gyro->Reset();
leftEnco->Reset();
rightEnco->Reset();
LEDLight->Set(Relay::kForward);
//Find out the type of autonomous we are using
int autonType = (int)SmartDashboard::GetNumber(NUM_BALL_AUTO);
if(autonType == 2)//Set the auton mode to two if we are doing a two ball auto
{
autonMode = TWO_BALL_AUTON;
autonStep = AUTON_TWO_WAIT_FOR_INTAKE;
}
else//Set the auton to one if the value on SD is set to 1 or another random value
{
autonMode = ONE_BALL_AUTON;
autonStep = AUTON_ONE_FIND_HOT;
}
//Bring the intake down
intake->DropIntake();
intakeDropTimer->Reset();
intakeDropTimer->Start();
while(IsAutonomous() && !IsDisabled())
{
rpi->Read();
lcd->Printf(DriverStationLCD::kUser_Line1, 1, "L: %f", leftEnco->GetDistance());
lcd->Printf(DriverStationLCD::kUser_Line2, 1, "R: %f", rightEnco->GetDistance());
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "T: %f", hotGoalTimer->Get());
lcd->Printf(DriverStationLCD::kUser_Line4, 1, "i: %i", rpi->GetMissingPacketcount());
lcd->Printf(DriverStationLCD::kUser_Line5, 1, "%i", rpi->GetXPos());
lcd->Printf(DriverStationLCD::kUser_Line6, 1, "%i", rpi->GetYPos());
lcd->UpdateLCD();
LEDLight->Set(Relay::kForward);
if(autonMode == ONE_BALL_AUTON)
{
switch(autonStep)
{
case AUTON_ONE_FIND_HOT:
//Reload the catapult and find the hot goal
rpi->Read();
if(!goalFound)
{
//This is put into an if statement to protect against the
//rpi finding the hot goal and then losing it
goalFound = ((rpi->GetXPos() != RPI_ERROR_VALUE) ||
(rpi->GetYPos() != RPI_ERROR_VALUE));
}
//Wait for the goal to be hot and the intake to move down
if(((goalFound) || (hotGoalTimer->Get() >= 6.75)) && intakeDropTimer->Get() >= INTAKE_DROP_WAIT)
{
autonStep = AUTON_ONE_SHOOT;
catapult->StartRelease();
}
break;
case AUTON_ONE_SHOOT:
//Shoot the catapult
if(!catapult->ReleaseHold())
{
//Move on to the next step when the catapult is released
autonStep = AUTON_ONE_WAIT;
//Start the wait timer
reloadTimer->Reset();
reloadTimer->Start();
}
break;
case AUTON_ONE_WAIT:
if(reloadTimer->Get() >= CATAPULT_WAIT_TIME)
{
autonStep = AUTON_ONE_DRIVE_FORWARDS;
reloadTimer->Stop();
//Start reloading the catapult
catapult->StartLoad();
gyro->Reset();
}
break;
case AUTON_ONE_DRIVE_FORWARDS:
//Drive forwards into the alliance zone and reload the catapult
if((!GyroDrive(0, 0.75, 36)) && (!(bool)catapult->Load()))
{
autonStep = AUTON_END;
}
break;
case AUTON_END:
break;
}
}
else if(autonMode == TWO_BALL_AUTON)
{
switch(autonStep)
{
/*case AUTON_TWO_RELOAD:
//Determine if the hot goal is on the right
if((rpi->GetXPos() != RPI_ERROR_VALUE) &&
(rpi->GetYPos() != RPI_ERROR_VALUE))
{
rightSideHot = true;
}
//Reload the catapult
if(!((bool)catapult->Load()))
{
autonStep = AUTON_TWO_FIRST_SHOOT;
catapult->StartShoot();
}
if((goalFound))
{
//If the goal is found, the right side is hot and we can go to the next step
rightSideHot = true;
autonStep = AUTON_TWO_FIRST_SHOOT;
}
else if(hotGoalTimer->Get() >= 0.5)
{
//If the timer runs of, the right side is not hot and we can go to the next step
rightSideHot = false;
autonStep = AUTON_TWO_FIRST_TURN;
}
break;*/
case AUTON_TWO_WAIT_FOR_INTAKE:
//wait for the intake to drop down
if(intakeDropTimer->Get() >= INTAKE_DROP_WAIT)
{
autonStep = AUTON_TWO_FIRST_SHOOT;
catapult->StartShoot();
}
break;
case AUTON_TWO_FIRST_SHOOT:
if(catapult->GetLoadingState() == LOAD_RELEASE_TENSION)
{
intake->RollIn();
}
if(!((bool)catapult->Shoot()))
{
autonStep = AUTON_TWO_INTAKE;
intakeTimer->Reset();
intakeTimer->Start();
}
break;
case AUTON_TWO_INTAKE:
intake->RollIn();
if(intakeTimer->Get() >= 1.5)
{
intake->Stop();
intakeTimer->Stop();
autonStep = AUTON_TWO_SECOND_SHOOT;
catapult->StartRelease();
}
break;
case AUTON_TWO_SECOND_SHOOT:
if(!catapult->ReleaseHold())
{
autonStep = AUTON_TWO_WAIT;
reloadTimer->Reset();
reloadTimer->Start();
}
break;
case AUTON_TWO_WAIT:
if(reloadTimer->Get() >= 0.5)
{
reloadTimer->Stop();
leftEnco->Reset();
rightEnco->Reset();
catapult->StartLoad();
gyro->Reset();
autonStep = AUTON_TWO_DRIVE_FORWARDS;
}
break;
case AUTON_TWO_DRIVE_FORWARDS:
if((!GyroDrive(0, 0.9, 36)) && (!((bool)catapult->Load())))
{
autonStep = AUTON_END;
}
break;
/*case AUTON_TWO_FIRST_TURN:
//Turn to the left 5* if the right goal is not hot
if(!rightSideHot)
{
if(!GyroTurn(-5, 0.5))
{
autonStep = AUTON_TWO_FIRST_SHOOT;
}
}
else
{
autonStep = AUTON_TWO_FIRST_SHOOT;
}
break;
case AUTON_TWO_FIRST_SHOOT:
//Release the catapult to shoot
if(!catapult->ReleaseHold())
{
autonStep = AUTON_TWO_FIRST_WAIT;
reloadTimer->Reset();
reloadTimer->Start();
}
break;
case AUTON_TWO_FIRST_WAIT:
if(reloadTimer->Get() >= CATAPULT_WAIT_TIME)
{
autonStep = AUTON_TWO_SECOND_TURN;
catapult->StartLoad();
reloadTimer->Stop();
}
break;
case AUTON_TWO_SECOND_TURN:
//Turn the robot so it's facing forwards and reload the catapult
if((!GyroTurn(0, 0.5)) && (!(bool)catapult->Load()))
{
autonStep = AUTON_TWO_GET_SECOND_BALL;
}
break;
case AUTON_TWO_GET_SECOND_BALL:
//Start up the intake and drive back to pick up the second ball
intake->RollIn();
if(!GyroDrive(0, -0.5, -12))
{
autonStep = AUTON_TWO_THIRD_TURN;
leftEnco->Reset();
rightEnco->Reset();
}
break;
case AUTON_TWO_THIRD_TURN:
//If the right goal was originally hot, turn left
if(rightSideHot)
{
if(!GyroTurn(-5, 0.5))
{
autonStep = AUTON_TWO_SECOND_SHOOT;
}
}
else
{
autonStep = AUTON_TWO_SECOND_SHOOT;
}
break;
case AUTON_TWO_SECOND_SHOOT:
intake->Stop();
if(!catapult->ReleaseHold())
{
autonStep = AUTON_TWO_SECOND_WAIT;
reloadTimer->Reset();
reloadTimer->Start();
}
break;
case AUTON_TWO_SECOND_WAIT:
if(reloadTimer->Get() >= CATAPULT_WAIT_TIME)
{
autonStep = AUTON_TWO_DRIVE_FORWARDS;
catapult->StartLoad();
reloadTimer->Stop();
}
break;
case AUTON_TWO_DRIVE_FORWARDS:
if(!GyroDrive(0, 0.75, 60) && (!(bool)catapult->Load()))
{
autonStep = AUTON_END;
}
break;*/
case AUTON_END:
break;
}
}
}
}
int main(void) {
int enc_now[2] = {},
enc_old[2] = {},
push_time[3] = {},
_build_mode = 0;
float yaw_now = 0,
yaw_old = 0;
bool Start = false;
//Initialise Systick
MillisecondTimer::initialise();
Nvic::initialise();
MainV3 mainBoard;
SpiMotor spimotor( *mainBoard.spi3v1, 1);
MillisecondTimer::delay(100);
PS3Con *ps3con = new PS3Con();
mainBoard.can.AddListenerNode(ps3con);
mainBoard.mpu6050.setTimeout(20);
debug<<"[INFO]Testing MPU6050...\r\n";
while(!mainBoard.mpu6050.test());
debug<<"[INFO]MPU6050 test passed.\r\n";
debug<<"[INFO]Setting up MPU6050...\r\n";
mainBoard.mpu6050.setup();
mainBoard.mpu6050.setGyrRange(mainBoard.mpu6050.GYR_RANGE_500DPS);
debug<<"[INFO]Setup complete!\r\n";
SensorTimer sensor_timer(&mainBoard);
Machine machine;
Gyro gyro;
Builder build(&mainBoard,&machine);
//Reset
build.Reset();
while (1) {
int rotation_gyro = 0,
rotation = 0,
D_out = 0,
SPI_out = 0,
X_100 = 0,
Y_100 = 0;
float A_out = 0,
B_out = 0,
C_out = 0;
mainBoard.can.Update();
mainBoard.buzzer.stop();
//Safety start
if (Start == false || ps3con->getButtonPress(CONNECTED) == 0) {
if (ps3con->getButtonPress(START)){
Start = true;
debug << "[INFO]Safety launch success!\r\n";
}else{
debug << "[WARN]Please push START button.\r\n";
}
if (Start == true) mainBoard.buzzer.set(-1, 6);
else mainBoard.buzzer.flash();
mainBoard.led.flash();
build.changeMode(NO_CAHNGE);
D_out = build.pwmArm((int)mainBoard.ad[2]->get());
SPI_out = build.pwmPlate((int)mainBoard.ad[3]->get());
setLimit(D_out,300);
setLimit(SPI_out,200);
mainBoard.motorD.setOutput((float) D_out / 500.0);
spimotor.setOutput((float) SPI_out / 500.0);
MillisecondTimer::delay(50);
continue;
}else if(Start == true && ps3con->getButtonPress(START)){//Emergency stop
mainBoard.buzzer.set(-1, 4);
debug << "[WARN]Emergency stop!!\r\n";
Start = false;
build.Reset();
//PWM output
mainBoard.motorA.setOutput(0 / 500.0);
mainBoard.motorB.setOutput(0 / 500.0);
mainBoard.motorC.setOutput(0 / 500.0);
mainBoard.motorD.setOutput(0 / 500.0);
spimotor.setOutput(0/500.0);
MillisecondTimer::delay(200);
continue;
}
mainBoard.led.On();
enc_now[X] = mainBoard.encoders.getCounter1();
enc_now[Y] = mainBoard.encoders.getCounter2();
machine.extendEncValue(enc_now[X],enc_old[X],X);
machine.extendEncValue(enc_now[Y],enc_old[Y],Y);
sensor_timer.ahrs.getYaw(yaw_now);
yaw_now *= RAD_TO_DEG;
gyro.set(yaw_now,yaw_old);
rotation_gyro = gyro.correct();
setLimit(rotation_gyro,R_Gyro_LIMIT);
//Rotate (angle of 90)
if (ps3con->getButtonPress(R1) && push_time[0] == 0){
mainBoard.buzzer.set(-1, 5);
gyro.setAngle90(90);
push_time[0] = 1;
}
else if (ps3con->getButtonPress(L1) && push_time[0] == 0){
mainBoard.buzzer.set(-1, 5);
gyro.setAngle90(-90);
push_time[0] = 1;
}
//Rotate (Normal)
if(ps3con->getButtonPress(L2)||ps3con->getButtonPress(R2)){
gyro.reset(rotation_gyro);
rotation = (ps3con->getAnalog(ANALOG_R2) - ps3con->getAnalog(ANALOG_L2)) / 2;
}
machine.antiSlip(rotation,ROTATE,NO_CAHNGE);
setLimit(rotation,R_LIMIT);
//change manual build mode
if(ps3con->getButtonPress(SELECT) && push_time[1] == 0){
mainBoard.buzzer.set(-1, 5);
push_time[1] = 1;
if(_build_mode == AUTO){
_build_mode = MANUAL;
build.changeMode(-2);
}
else{
_build_mode = AUTO;
build.changeMode(-6);
}
}
//Build capital (push_time)
if(ps3con->getButtonPress(CIRCLE) && push_time[2] == 0){
mainBoard.buzzer.set(-1, 5);
push_time[2] = 1;
if(_build_mode == AUTO){
build.changeMode();
}else if(_build_mode == MANUAL){
if(build.getMode() == -2) build.changeMode(1);//Get
else build.changeMode(-2);//Release
}
build.resetPause();
}else if(ps3con->getButtonPress(CROSS) && push_time[2] == 0 && _build_mode == AUTO){
mainBoard.buzzer.set(-1, 4);
push_time[2] = 1;
build.changeMode(build.getMode() - 1);
build.resetPause();
}
build.changeMode(NO_CAHNGE);
for(int i = 0;i < 3;i++){
if(push_time[i] != 0){
push_time[i]++;
if(push_time[i] > 10) push_time[i] = 0;
}
}
X_100 = ps3con->convertValue(ps3con->getAnalog(ANALOG_L_X),build.getGain());
Y_100 = ps3con->convertValue(ps3con->getAnalog(ANALOG_L_Y),build.getGain());
//Adjust bit
if(ps3con->getButtonPress(TRIANGLE)){
if (ps3con->getButtonPress(RIGHT)){ if(X_100 < SPEED_Linearly) X_100 += 20;}
else if (ps3con->getButtonPress(LEFT)){ if(X_100 > -SPEED_Linearly) X_100 -= 20;}
else if (ps3con->getButtonPress(UP)){ if(Y_100 > -SPEED_Linearly) Y_100 -= 20;}
else if (ps3con->getButtonPress(DOWN)){ if(Y_100 < SPEED_Linearly) Y_100 += 20;}
else{ X_100 = 0; Y_100 = 0; }
}
//Automatically adjust the distance between the object
if (ps3con->getButtonPress(SQUARE) && ps3con->getButtonPress(UP)) {
Y_100 = machine.adjustDistance((int)mainBoard.ad[0]->get(),build.getMode(),Y);
if(Y_100 == 0) mainBoard.buzzer.set(-1, 6);
}
//Automatically adjust the distance between the wall
if (ps3con->getButtonPress(SQUARE) && ps3con->getButtonPress(LEFT)) {
X_100 = machine.adjustDistance((int)mainBoard.ad[1]->get(),build.getMode(),X);
if(X_100 == 0) mainBoard.buzzer.set(-1, 6);
}
//Automatically run to target
// if (ps3con->getButtonPress(CROSS)) {
// X_100 = machine.runToTarget(X);
// Y_100 = machine.runToTarget(Y);
//
// if(X_100 == 0 && Y_100 == 0) mainBoard.buzzer.set(-1, 6);
// }
X_100 = machine.antiSlip(X_100, X,build.getMode());
Y_100 = machine.antiSlip(Y_100, Y,build.getMode());
//motor
A_out = X_100 / 2 - Y_100 * sqrt(3) / 2 + rotation + rotation_gyro;
B_out = X_100 / 2 + Y_100 * sqrt(3) / 2 + rotation + rotation_gyro;
C_out = -X_100 + rotation + rotation_gyro;
//switch Arm & Plate mode
if(_build_mode == AUTO){
D_out = build.pwmArm((int)mainBoard.ad[2]->get());
SPI_out = build.pwmPlate((int)mainBoard.ad[3]->get());
}else{
int dust = 0;//no function
dust = build.pwmArm((int)mainBoard.ad[2]->get());
dust = build.pwmPlate((int)mainBoard.ad[3]->get());
D_out = -ps3con->convertValue(ps3con->getAnalog(ANALOG_R_Y),5);
}
//debug
//SPI_out = -ps3con->convertValue(ps3con->getAnalog(ANALOG_R_Y),2);
//D_out = -ps3con->convertValue(ps3con->getAnalog(ANALOG_R_Y),5);
setLimitFloat(A_out,PWM_LIMIT);
setLimitFloat(B_out,PWM_LIMIT);
setLimitFloat(C_out,PWM_LIMIT);
setLimit(D_out,PWM_LIMIT);
setLimit(SPI_out,110);
//PWM output
mainBoard.motorA.setOutput(A_out / 500.0);
mainBoard.motorB.setOutput(B_out / 500.0);
mainBoard.motorC.setOutput(C_out / 500.0);
mainBoard.motorD.setOutput((float)D_out / 500.0);
spimotor.setOutput((float)SPI_out/500.0);
yaw_old = yaw_now;
enc_old[X] = enc_now[X];
enc_old[Y] = enc_now[Y];
//debug
char str[128];
sprintf(str, "[DEBUG]Mode:%d,%d Arm,Plate:%s,%s PS3con:%d,%d omniPWM:%.0f,%.0f,%.0f Gyro:%d Rotate:%d Arm:%d Plate:%d\r\n"
,build.getMode(),_build_mode,build.getComp(ARM)?"O":"X",build.getComp(PLATE)?"O":"X"
,X_100,Y_100,A_out,B_out,C_out,rotation_gyro,rotation,(int)mainBoard.ad[2]->get(),(int)mainBoard.ad[3]->get());
//sprintf(str,"[TEST]%d %d\r\n",(int)mainBoard.ad[3]->get(),SPI_out);
//sprintf(str,"[TEST]Front:%d Left:%d\r\n",(int)mainBoard.ad[0]->get(),(int)mainBoard.ad[1]->get());
debug << str;
MillisecondTimer::delay(50);
}
}
