From 66c6c761041de2e1fb836f27bd89a39296273728 Mon Sep 17 00:00:00 2001 From: Austen Adler Date: Sat, 15 Feb 2014 15:47:49 +0000 Subject: [PATCH] Added zaphod code --- MyRobot.cpp | 497 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 497 insertions(+) create mode 100644 MyRobot.cpp diff --git a/MyRobot.cpp b/MyRobot.cpp new file mode 100644 index 0000000..25f50d3 --- /dev/null +++ b/MyRobot.cpp @@ -0,0 +1,497 @@ +//TODO: +//Vim-like command propmpt thingy +//about line 334 +//Auto +//Sonar code +#include "WPILib.h" +#include "SmartDashboard/SmartDashboard.h" +#include "Command.h" +#include +#include +class RobotDemo : public SimpleRobot +{ + RobotDrive myRobot; + //Joystick button inputs + int buttonOneState, buttonTwoState, buttonFiveState, buttonSixState, buttonEightState, buttonSevenState, buttonNineState, buttonTenState, buttonTwelveState, LbuttonOneState, LbuttonTwoState, LbuttonSevenState, LbuttonEightState,LbuttonNineState, LbuttonTenState, LbuttonElevenState, LbuttonTwelveState; + float potVal,multiplier, servoXState, servoYState, x, y, throttle, ServoXJoyPos, ServoYJoyPos; + int leftPower, rightPower, cameraMode; + int cameraPreset, collectorSpeed; + bool collectorExtended,toggleCollector; + int lastToggle; + bool compressorb, compressing; + float DownSpeed; + string cmd, debug; + Joystick Rstick, Lstick; + Servo Servo1, Servo2; + Compressor compressor; + Solenoid collectorSole1, collectorSole2; + DigitalInput pneumaticLS, shootTopLS, shootBottomLS; + Relay collectorSpike, lightingSpike; + Jaguar Left1,Left2,Left3,Right1,Right2,Right3, RightArmMotor1,RightArmMotor2, LeftArmMotor1,LeftArmMotor2,CollectorMotor1; + //Pot + AnalogChannel armPot; + //Ultrasonic + AnalogChannel BallSonicLeft,BallSonicRight,WallSonicLeft,WallSonicRight; +public: + RobotDemo(): + //Joysticks + Rstick(1), + Lstick(2), + //Pot + armPot(5), + //Ultrasonic + BallSonicLeft(1), + BallSonicRight(2), + WallSonicLeft(3), + WallSonicRight(4), + //Compressor + //compressor(2,3,1,1), + compressor(2,5,1,1),//Solenoids + collectorSole1(1), + collectorSole2(2), + //Limit switches + pneumaticLS(1), + shootTopLS(2), + shootBottomLS(3), + //Driver Motors + Left1(1,1), + Left2(1,2), + Left3(1,3), + Right1(2,1), + Right2(2,2), + Right3(2,3), + //Servos + Servo1(1,7), + Servo2(1,8), + //Spikes + collectorSpike(2,7), + lightingSpike(2,8), + //Shooter Motors + LeftArmMotor1(1,4), + LeftArmMotor2(1,5), + RightArmMotor1(2,4), + RightArmMotor2(2,5), + //Collector Motor + CollectorMotor1(1,6), + myRobot(Left1,Left2,Right1,Right2) { + GetWatchdog().SetEnabled(false); + } + void RobotInit() { + //Initializing robot + lastToggle = 0; + DashboardSetup(); + cameraMode = 0; + servoXState = 90; + servoYState = 90; + multiplier = 1.0f; + cameraPreset = 0; + compressor.Start(); + compressorb = true; + compressing = true; + toggleCollector=false; + debug=""; + } + void DashboardSetup() { + SmartDashboard::PutNumber("Throttle", throttle); + SmartDashboard::PutNumber("DownSpeed", 0.100); + SmartDashboard::PutString("Auto", cmd); + SmartDashboard::PutString("Debuggin:", debug); + SmartDashboard::PutNumber("collectorSpeed",127); + SmartDashboard::PutNumber("armPot", armPot.GetAverageVoltage()*10000.0f); + //FOR DA SAMPLE TEXT BOT POT + //min: 0.04803774 + //max: 0.05024723 + //(#-0.04803774)/0.05024723 + //Ultrasonic + SmartDashboard::PutNumber("Wall Left",voltToDistance(WallSonicLeft.GetAverageVoltage())); + SmartDashboard::PutNumber("Wall Right",voltToDistance(WallSonicRight.GetAverageVoltage())); + SmartDashboard::PutNumber("Ball Left",voltToDistance(BallSonicLeft.GetAverageVoltage())); + SmartDashboard::PutNumber("Ball Right",voltToDistance(BallSonicRight.GetAverageVoltage())); + } + void updateDashboard() { + SmartDashboard::PutNumber("Throttle", throttle); + collectorSpeed = SmartDashboard::GetNumber("collectorSpeed"); + SmartDashboard::PutNumber("armPot", armPot.GetAverageVoltage()*10000.0f); + //Ultrasonic + SmartDashboard::PutNumber("Wall Left",voltToDistance(WallSonicLeft.GetAverageVoltage())); + SmartDashboard::PutNumber("Wall Right",voltToDistance(WallSonicRight.GetAverageVoltage())); + SmartDashboard::PutNumber("Ball Left",voltToDistance(BallSonicLeft.GetAverageVoltage())); + SmartDashboard::PutNumber("Ball Right",voltToDistance(BallSonicRight.GetAverageVoltage())); + SmartDashboard::PutNumber("Tanval",tan((WallSonicLeft.GetAverageVoltage()-WallSonicRight.GetAverageVoltage())/18.0f)); + DownSpeed = SmartDashboard::GetNumber("DownSpeed"); + } + void buttonUpdate() { + //Right joystick input + buttonOneState=Rstick.GetRawButton(1); + buttonTwoState=Rstick.GetRawButton(2); + buttonFiveState=Rstick.GetRawButton(5); + buttonSixState=Rstick.GetRawButton(6); + buttonSevenState=Rstick.GetRawButton(7); + buttonEightState=Rstick.GetRawButton(8); + buttonNineState=Rstick.GetRawButton(9); + buttonTenState=Rstick.GetRawButton(10); + buttonTwelveState=Rstick.GetRawButton(12); + //Left joystick input + LbuttonOneState=Lstick.GetRawButton(1); + LbuttonTwoState=Lstick.GetRawButton(2); + LbuttonSevenState=Lstick.GetRawButton(7); + LbuttonEightState=Lstick.GetRawButton(8); + LbuttonNineState=Lstick.GetRawButton(9); + LbuttonTenState=Lstick.GetRawButton(10); + LbuttonElevenState=Lstick.GetRawButton(11); + LbuttonTwelveState=Lstick.GetRawButton(12); + } + void getJoystickAxis() { + y=Rstick.GetX()*multiplier; + x=-Rstick.GetY()*multiplier; + throttle=(-Lstick.GetRawAxis(4)+1.0f)/2.0f; + leftPower=((y+x)/2+1)*127+1; + rightPower=((y-x)/2+1)*127+1; + } + void updateMotors() { + setMotorValue(1,1,leftPower); + setMotorValue(2,1,leftPower); + setMotorValue(3,1,leftPower); + setMotorValue(1,2,rightPower); + setMotorValue(2,2,rightPower); + setMotorValue(3,2,rightPower); + } + //camera functions + void camerafaceManual(int changex,int changey) { + servoYState=changey; + servoXState=changex; + setMotorValue(7,2,servoXState); + Wait(0.003); + setMotorValue(8,2,servoYState); + } + void cameraReset() { + servoXState = 90; + servoYState = 90; + setMotorValue(7,2,servoXState); + setMotorValue(8,2,servoYState); + } + float voltToDistance(float a) { + return a/0.00488f/2.54f; + } + int cvt(float input) { + return input*127.0f+128; + } + void setDigitalValue(int device,int subwayStation=1,int value=127) { + if(subwayStation==1) { + switch(device) { + case 1: + break; + case 2: + break; + case 3: + break; + case 4: + break; + case 5: + break; + case 6: + break; + case 7: + break; + case 8: + break; + case 9: + break; + case 10: + break; + case 11: + break; + case 12: + break; + case 13: + break; + case 14: + break; + default: + break; + } + } else if(subwayStation==2) { + switch(device) { + case 1: + break; + case 2: + break; + case 3: + break; + case 4: + break; + case 5: + break; + case 6: + break; + case 7: + break; + case 8: + break; + case 9: + break; + case 10: + break; + case 11: + break; + case 12: + break; + case 13: + break; + case 14: + break; + default: + break; + } + } + } + void setMotorValue(int motor,int subwayStation=1,int value=127) { + if(subwayStation==1) { + switch(motor) { + //Drive motors + case 1: + Left1.SetRaw(value); + break; + case 2: + Left2.SetRaw(value); + break; + case 3: + Left3.SetRaw(value); + break; + case 4: + LeftArmMotor1.SetRaw(value); + break; + case 5: + LeftArmMotor2.SetRaw(value); + break; + case 6: + CollectorMotor1.SetRaw(value); + break; + case 7: + Servo1.SetAngle(value); + break; + case 8: + Servo2.SetAngle(value); + break; + case 9: + break; + case 10: + break; + } + } else if(subwayStation==2) { + switch(motor) { + //Shooter motors + case 1: + Right1.SetRaw(value); + break; + case 2: + Right2.SetRaw(value); + break; + case 3: + Right3.SetRaw(value); + break; + case 4: + RightArmMotor1.SetRaw(value); + break; + case 5: + RightArmMotor2.SetRaw(value); + break; + case 6: + break; + case 7: + break; + case 8: + break; + case 9: + break; + case 10: + break; + } + } + } + bool checkSafety() { + //Some function that will be called to make sure the collector is out before the shooter is raised + if(collectorExtended == true) { + debug += "Collector is extended, not going to fire\n"; + return false; + } + if(collectorExtended == false) { + debug += "Collector is NOT extended, going to fire\n"; + return true; + } + } + void alignRobot() { + float yoyo=atan(voltToDistance(WallSonicLeft.GetAverageVoltage())-voltToDistance(WallSonicRight.GetAverageVoltage()))/18.0f; + SmartDashboard::PutNumber("Distance", yoyo); + } + //Active functions + void Test() { + //code for sonar + int i=0; + while(IsEnabled()&&IsTest()) { + i++; + if(i%250==0) { + SmartDashboard::PutNumber("armPot", armPot.GetAverageVoltage()*1000.0f); + } + } + } + void Autonomous() { + myRobot.SetSafetyEnabled(false); + compressor.Start(); + cmd=SmartDashboard::GetString("Auto"); + vector cmds = getCommands((std::string)cmd); + //printf(((std::string)cmd).c_str()); + //10d drive 10 feet + //10w wait 10 seconds + //90t turn 90 degrees, yo + //3c collect ball and retract after 3 seconds + //255s shoot at power 255 + //3a align robit for 3 seconds (uses ultrasonic to align robit) + int commandIndex = 0; + int i = 0; + while(IsEnabled()&&IsAutonomous()) { + if(commandIndex < cmds.size()) { + if(cmds[commandIndex].Function == "d") { + if(i*1000 < cmds[commandIndex].Amount) { + setMotorValue(4,1,255); + setMotorValue(5,1,255); + setMotorValue(4,2,255); + setMotorValue(5,2,255); + } else { + commandIndex++; + i=0; + } + } else if(cmds[commandIndex].Function == "w") { + if(!(i*1000 < cmds[commandIndex].Amount)) { + commandIndex++; + i=0; + } + } + } + potVal=420.0f; + updateDashboard(); + if(compressor.GetPressureSwitchValue()==1) { + compressor.Stop(); + } + i++; + Wait(0.005f); + } + compressor.Stop(); + } + void OperatorControl() { + myRobot.SetSafetyEnabled(false); + int i=0; + collectorSole1.Set(true); + collectorSole2.Set(false); + compressor.Start(); + while(IsEnabled()&&IsOperatorControl()) { + getJoystickAxis(); + //Log things + if(i%500==0) { + printf("Throttle values: (%f,%f,%f)\n", throttle,128-(throttle*127.0f),(throttle*127.0f)+128); + printf("X,Y: (%f,%f)\n", x, y); + printf("Pitchfork values: (%f,%f)\n", leftPower, rightPower); + } + if(compressing && compressor.GetPressureSwitchValue()) { + compressor.Stop(); + compressing = false; + printf("Stopping the compressor\n"); + } + if(!compressing && compressor.GetPressureSwitchValue()==0) { + compressor.Start(); + compressing=true; + printf("Starting the compressor again\n"); + Wait(0.05f); + } + updateMotors(); + buttonUpdate(); + updateDashboard(); + if(LbuttonSevenState) { + cameraPreset++; + if(cameraPreset>3) { + cameraPreset=0; + } + switch(cameraPreset) { + case 0: + camerafaceManual(-90,0); + break; + case 1: + camerafaceManual(90,0); + break; + case 2: + camerafaceManual(0,-90); + break; + case 3: + camerafaceManual(0,90); + break; + } + Wait(.0005f); + } + if(LbuttonOneState==1) { + //Move arm motors based on throttle + if(collectorExtended == false) { + debug += "Collector is extended, not going to fire\n"; + } + if(collectorExtended == true) { + setMotorValue(4,1,cvt(throttle)); + setMotorValue(5,1,cvt(throttle)); + setMotorValue(4,2,cvt(-throttle)); + setMotorValue(5,2,cvt(-throttle)); + debug += "Collector is NOT extended, going to fire\n"; + } + } else if(LbuttonTwoState==1) { + //Reverse the arm motors + if(collectorExtended == false) { + debug += "Collector is not extended, not going to fire\n"; + } + if(collectorExtended == true) { + setMotorValue(4,1,-cvt(DownSpeed)); + setMotorValue(5,1,-cvt(DownSpeed)); + setMotorValue(4,2,cvt(DownSpeed)); + setMotorValue(5,2,cvt(DownSpeed)); + debug += "Collector is extended, going to fire\n"; + } + } else { + //Stop all motors + setMotorValue(4,1,127); + setMotorValue(5,1,127); + setMotorValue(4,2,127); + setMotorValue(5,2,127); + } + + if(LbuttonTenState) { + collectorExtended = true; + collectorSole1.Set(false); + collectorSole2.Set(true); + + } else if(LbuttonNineState) { + collectorExtended = false; + collectorSole1.Set(true); + collectorSole2.Set(false); + } + if(LbuttonEightState&&i-lastToggle>200) { + lastToggle=i; + collectorExtended = !collectorExtended; + collectorSole1.Set(collectorExtended); + collectorSole2.Set(!collectorExtended); + //setMotorValue(6,1,collectorExtended?); + } + if(LbuttonElevenState) { + setMotorValue(6,1,1); + } else if(LbuttonTwelveState) { + setMotorValue(6,1,255); + } else { + setMotorValue(6,1,0); + } + //Camera directions + camerafaceManual(LbuttonTwelveState-LbuttonElevenState,LbuttonEightState-LbuttonTenState); + if(LbuttonNineState==1) { + cameraReset(); + } + i++; + Wait(0.005f); + } + } +}; +START_ROBOT_CLASS(RobotDemo); +