team2059/zaphod
2
0
Fork 0
mirror of https://github.com/team2059/Zaphod synced 2025-01-07 22:14:14 -05:00
Code Issues Releases Wiki Activity

Made changes to autonomous, added a sequence changer

Browse Source
This commit is contained in:
Austen Adler 2014-03-08 22:10:33 +00:00
parent 85d334f0fa
commit 9f14e2dbd2
1 changed files with 332 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

451
MyRobot.cpp
View File

@ -1,13 +1,16 @@
//Add a button on joystick that activates "auto" to drive to 40 inches away and another to shoot when at 40 inches away (use the little joystick on both drive and shooter stick) //Add a button on joystick that activates "auto" to drive to 40 inches away and another to shoot when at 40 inches away (use the little joystick on both drive and shooter stick)
//Sonar in auto: drive till 40in away (dashboard value) and shoot //Sonar in auto: drive till 40in away (dashboard value) and shoot
//Includes{{{
#include "WPILib.h" #include "WPILib.h"
#include "SmartDashboard/SmartDashboard.h" #include "SmartDashboard/SmartDashboard.h"
#include <iostream> #include <iostream>
#include <math.h> #include <math.h>
#include <vector> #include <vector>
#include <sstream> #include <sstream>
//}}}
class RobotDemo : public SimpleRobot class RobotDemo : public SimpleRobot
{ {
//Declarations{{{
RobotDrive myRobot; RobotDrive myRobot;
float potVal, multiplier, throttle; float potVal, multiplier, throttle;
bool collectorExtended, shooting, compressing, allowCompressing; bool collectorExtended, shooting, compressing, allowCompressing;
@ -19,8 +22,10 @@ class RobotDemo : public SimpleRobot
AnalogChannel armPot; AnalogChannel armPot;
AnalogChannel BallSonicLeft, BallSonicRight, WallSonicLeft, WallSonicRight; AnalogChannel BallSonicLeft, BallSonicRight, WallSonicLeft, WallSonicRight;
DigitalOutput BallLeft, BallRight, WallLeft, WallRight; DigitalOutput BallLeft, BallRight, WallLeft, WallRight;
//}}}
public: public:
RobotDemo(): RobotDemo():
//Initializations{{{
//Joysticks //Joysticks
Rstick(1), Rstick(1),
Lstick(2), Lstick(2),
@ -57,6 +62,8 @@ public:
myRobot(Left1, Left2, Right1, Right2) { myRobot(Left1, Left2, Right1, Right2) {
GetWatchdog().SetEnabled(false); GetWatchdog().SetEnabled(false);
} }
//}}}
//RobotInit{{{
void RobotInit() { void RobotInit() {
DashboardSetup(); DashboardSetup();
multiplier = 1.0f; multiplier = 1.0f;
@ -67,6 +74,8 @@ public:
allowCompressing = true; allowCompressing = true;
throttle=0; throttle=0;
} }
//}}}
//DashboardSetup{{{
void DashboardSetup() { void DashboardSetup() {
SmartDashboard::PutNumber("Throttle", throttle); SmartDashboard::PutNumber("Throttle", throttle);
SmartDashboard::PutNumber("upLimit", 120.0f); SmartDashboard::PutNumber("upLimit", 120.0f);
@ -81,11 +90,16 @@ public:
SmartDashboard::PutNumber("AutoPower",0.455f); SmartDashboard::PutNumber("AutoPower",0.455f);
SmartDashboard::PutNumber("AutoCorrection",0.06f); SmartDashboard::PutNumber("AutoCorrection",0.06f);
SmartDashboard::PutNumber("Inital Drive Timeout", 2.5f); SmartDashboard::PutNumber("Inital Drive Timeout", 2.5f);
SmartDashboard::PutNumber("First Shot Start", 2.5f); SmartDashboard::PutNumber("First Shot Start", 0.5f);
SmartDashboard::PutNumber("First Shot Stop", 0.5f); SmartDashboard::PutNumber("First Shot Stop", 1.0f);
SmartDashboard::PutNumber("Reverse direction start",3.5.0f); SmartDashboard::PutNumber("Reverse direction start",0.0f);
SmartDashboard::PutNumber("Reverse direction stop",5.5.0f); SmartDashboard::PutNumber("Reverse direction stop",2.5f);
SmartDashboard::PutNumber("Second Drive Start", 1.0f);
SmartDashboard::PutNumber("Second Drive Timeout", 2.5f);
SmartDashboard::PutNumber("Second Shot Start", 0.5f);
SmartDashboard::PutNumber("Second Shot Stop", 1.0f);
SmartDashboard::PutNumber("Autonomous step",0.0f); SmartDashboard::PutNumber("Autonomous step",0.0f);
SmartDashboard::PutNumber("Autonomous sequence", 0.0f);
//Shooter presets //Shooter presets
SmartDashboard::PutNumber("ShortRange",0.465f); //Power for the shooter when against the low goal SmartDashboard::PutNumber("ShortRange",0.465f); //Power for the shooter when against the low goal
SmartDashboard::PutNumber("ShooterButtonPower10",0.605f); SmartDashboard::PutNumber("ShooterButtonPower10",0.605f);
@ -101,6 +115,8 @@ public:
SmartDashboard::PutBoolean("Ignore Pot",false); SmartDashboard::PutBoolean("Ignore Pot",false);
//Battery voltage //Battery voltage
} }
//}}}
//updateDashboard{{{
void updateDashboard() { void updateDashboard() {
SmartDashboard::PutNumber("Throttle", throttle); SmartDashboard::PutNumber("Throttle", throttle);
SmartDashboard::PutNumber("armPot", potToDegrees(armPot.GetAverageVoltage())); SmartDashboard::PutNumber("armPot", potToDegrees(armPot.GetAverageVoltage()));
@ -115,42 +131,52 @@ public:
upLimit = 167; upLimit = 167;
} }
} }
void shootRobot(float power=0) { //}}}
setMotorValue(4, 1, cvt(power)); //shootRobot{{{
setMotorValue(5, 1, cvt(power)); void shootRobot(float power=0) {
setMotorValue(4, 2, cvt(-power)); setMotorValue(4, 1, cvt(power));
setMotorValue(5, 2, cvt(-power)); setMotorValue(5, 1, cvt(power));
} setMotorValue(4, 2, cvt(-power));
setMotorValue(5, 2, cvt(-power));
}
//}}}
//logMsg{{{
void logMsg(std::string message, int level) { void logMsg(std::string message, int level) {
if((int)SmartDashboard::GetNumber("Log Level") % level == 0) { if((int)SmartDashboard::GetNumber("Log Level") % level == 0) {
printf((message+"\n").c_str()); printf((message+"\n").c_str());
} }
} }
void driveRobot(float x, float y) { //}}}
if(y>1.0f) { //toString{{{
y=1.0f; template<typename numbertype> string toString(numbertype a) {
} else if(y!=0.0f&&y<-1.0f) { stringstream ss;
y=-1.0f; ss<<a;
string s = ss.str();
return s;
} }
int leftPower = ((y+x)/2+1)*127+1; //}}}
int rightPower = ((y-x)/2+1)*127+1; //driveRobot{{{
//logMsg("leftPower: "+toString<int>(leftPower),3); void driveRobot(float x, float y) {
//logMsg("rightPower: "+toString<int>(rightPower),3); if(y>1.0f) {
//logMsg("JoyX: "+toString<float>(Rstick.GetX()),3); y=1.0f;
//logMsg("JoyY: "+toString<float>(Rstick.GetY()),3); } else if(y!=0.0f&&y<-1.0f) {
setMotorValue(1, 1, leftPower); y=-1.0f;
setMotorValue(2, 1, leftPower); }
setMotorValue(3, 1, leftPower); int leftPower = ((y+x)/2+1)*127+1;
setMotorValue(1, 2, rightPower); int rightPower = ((y-x)/2+1)*127+1;
setMotorValue(2, 2, rightPower); //logMsg("leftPower: "+toString<int>(leftPower),3);
setMotorValue(3, 2, rightPower); //logMsg("rightPower: "+toString<int>(rightPower),3);
} //logMsg("JoyX: "+toString<float>(Rstick.GetX()),3);
template<typename numbertype> string toString(numbertype a) { //logMsg("JoyY: "+toString<float>(Rstick.GetY()),3);
stringstream ss; setMotorValue(1, 1, leftPower);
ss<<a; setMotorValue(2, 1, leftPower);
string s = ss.str(); setMotorValue(3, 1, leftPower);
return s; setMotorValue(1, 2, rightPower);
} setMotorValue(2, 2, rightPower);
setMotorValue(3, 2, rightPower);
}
//}}}
//voltToDistance{{{
float voltToDistance(float a,bool wall=false) { float voltToDistance(float a,bool wall=false) {
if(wall) { if(wall) {
return (a / 0.00488f) / 2.54f; return (a / 0.00488f) / 2.54f;
@ -158,6 +184,8 @@ public:
return (a / 0.000976562f) / 25.4f; return (a / 0.000976562f) / 25.4f;
} }
} }
//}}}
//potToDegrees{{{
float potToDegrees(float a) { float potToDegrees(float a) {
float max = -.0003948; float max = -.0003948;
float min = 5.0245547; float min = 5.0245547;
@ -166,13 +194,17 @@ public:
max = max - max; //=0 max = max - max; //=0
return 300 - ((b + max) * (300 / min)); return 300 - ((b + max) * (300 / min));
} }
//}}}
//cvt{{{
int cvt(float input) { int cvt(float input) {
return input * 127.0f + 128; return input * 127.0f + 128;
} }
//}}}
//setMotorValue{{{
void setMotorValue(int motor, int subwayStation = 1, int value = 127) { void setMotorValue(int motor, int subwayStation = 1, int value = 127) {
if(subwayStation == 1) { if(subwayStation == 1) {
//subwayStation1{{{
switch(motor) { switch(motor) {
//Drive motors
case 1: case 1:
Left1.SetRaw(value); Left1.SetRaw(value);
break; break;
@ -200,7 +232,9 @@ public:
case 10: case 10:
break; break;
} }
//}}}
} else if(subwayStation == 2) { } else if(subwayStation == 2) {
//subwayStation2{{{
switch(motor) { switch(motor) {
//Shooter motors //Shooter motors
case 1: case 1:
@ -229,15 +263,20 @@ public:
case 10: case 10:
break; break;
} }
//}}}
} }
} }
//}}}
//Test{{{
void Test() { void Test() {
} }
//}}}
//Autonomous{{{
void Autonomous() { void Autonomous() {
//Initializations{{{
myRobot.SetSafetyEnabled(false); myRobot.SetSafetyEnabled(false);
int i=0; int i=0;
int c=0; int c=0;
//TODO delete this and implement it
float getSecondBallStart=(SmartDashboard::GetNumber("Reverse direction start"))*200; float getSecondBallStart=(SmartDashboard::GetNumber("Reverse direction start"))*200;
float getSecondBallStop=(SmartDashboard::GetNumber("Reverse direction stop"))*200; float getSecondBallStop=(SmartDashboard::GetNumber("Reverse direction stop"))*200;
float power=SmartDashboard::GetNumber("AutoPower"); float power=SmartDashboard::GetNumber("AutoPower");
@ -251,34 +290,214 @@ public:
WallRight.Set(1); WallRight.Set(1);
BallRight.Set(0); BallRight.Set(0);
SmartDashboard::PutBoolean("CollectorState",true); SmartDashboard::PutBoolean("CollectorState",true);
//}}}
while(IsEnabled()&&IsAutonomous()) { while(IsEnabled()&&IsAutonomous()) {
if(currentStep==0){ //The first statement to check for. Will run at start because i-c will be 0 (shouldn't need to be changed) if(SmartDashboard::GetNumber("Autonomous Sequence")==0){
//Displays the step that is currently running in Autonomous //Autonomous0{{{
SmartDashboard::PutNumber("Autonomous step", currentStep); //Drive{{{
if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=40.0f){ //Particular only to the first drive step if(currentStep==0){
driveRobot(1.0f,correction); if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=40.0f){
driveRobot(1.0f,correction);
}else{
driveRobot(0.0f,0.0f);
currentStep=1;
c=0;
}
if(c==SmartDashboard::GetNumber("Inital Drive Timeout")*200){
driveRobot(0.0f,0.0f);
currentStep++;
c=0;
}
} }
if(c==(SmartDashboard::GetNumber("Inital Drive Timeout"))*200){ //Acts as the else (ie: motor stopping, etc) //}}}
//Shoot{{{
if(currentStep==1&&c>SmartDashboard::GetNumber("First Shot Start")*200){
if(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage())){
shootRobot(power);
}else{
shootRobot(0.0f);
}
if(c==(SmartDashboard::GetNumber("First Shot Stop"))*200){
shootRobot(0.0f);
currentStep++;
c=0;
}
}
//}}}
//Lower Shooter{{{
if(currentStep==2&&c>SmartDashboard::GetNumber("Reverse direction start")*200){
driveRobot(-1.0f,correction);
if(40.0f<=potToDegrees(armPot.GetAverageVoltage())){
driveRobot(0.0f,0.0f);
shootRobot(-0.1f);
currentStep++;
c=0;
}
}
//}}}
//}}}
}else(SmartDashboard::GetNumber("Autonomous Sequence")==1){
//Autonomous1{{{
//Drive{{{
if(currentStep==0){
if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=40.0f){
driveRobot(1.0f,correction);
}else{
driveRobot(0.0f,0.0f);
currentStep=1;
c=0;
}
if(c==SmartDashboard::GetNumber("Inital Drive Timeout")*200){
driveRobot(0.0f,0.0f);
currentStep++;
c=0;
}
}
//}}}
//Shoot{{{
if(currentStep==1&&c>SmartDashboard::GetNumber("First Shot Start")*200){
if(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage())){
shootRobot(power);
}else{
shootRobot(0.0f);
}
if(c==(SmartDashboard::GetNumber("First Shot Stop"))*200){
shootRobot(0.0f);
currentStep++;
c=0;
}
}
//}}}
//Drive Backwards{{{
if(currentStep==2&&c>SmartDashboard::GetNumber("Reverse direction start")*200){
driveRobot(-1.0f,correction);
if(c==(SmartDashboard::GetNumber("Reverse direction stop"))*200){
driveRobot(0.0f,0.0f);
shootRobot(-0.1f);
currentStep++;
c=0;
}
}
//}}}
//Drive{{{
if(currentStep==3&&c>SmartDashboard::GetNumber("Second Drive Start")*200){
if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=40.0f){
driveRobot(1.0f,correction);
}else{
driveRobot(0.0f,0.0f);
currentStep++;
c=0;
}
if(c==(SmartDashboard::GetNumber("Inital Drive Timeout"))*200){
driveRobot(0.0f,0.0f);
currentStep++;
c=0;
}
}
//}}}
//Shoot{{{
if(currentStep==4&&c>(SmartDashboard::GetNumber("Second Shot Start"))*200){
if(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage())){
shootRobot(power);
}else{
shootRobot(0.0f);
}
if(c==(SmartDashboard::GetNumber("First Shot Stop"))*200){
shootRobot(0.0f);
currentStep++;
c=0;
}
}
//}}}
//}}}
}else(SmartDashboard::GetNumber("Autonomous Sequence")==2){
//Autonomous2{{{
//Drive{{{
if(currentStep==0){
if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=40.0f){
driveRobot(1.0f,correction);
shootRobot(0.0f);
setMotorValue(6, 1, 0);
}else{
driveRobot(0.0f,0.0f);
shootRobot(0.0f);
setMotorValue(6, 1, 0);
currentStep++;
c=0;
}
if(c==SmartDashboard::GetNumber("Inital Drive Timeout")*200){
driveRobot(0.0f,0.0f);
currentStep++;
c=0;
}
}
//}}}
//Release Ball{{{
if(currentStep==1){
setMotorValue(6, 1, 102);
shootRobot(0.0f);
driveRobot(0.0f,0.0f); driveRobot(0.0f,0.0f);
currentStep=1; if(c==50){
c=0; //Reset the timer currentStep++;
} c=0;
} }
if(currentStep==1&&c>(SmartDashboard::GetNumber("First Shot Start"))*200){ //The next 'step' in auto, shooting the motors when startShootingFirst is true
//Displays the step that is currently running in Autonomous
SmartDashboard::PutNumber("Autonomous step", currentStep);
if(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage())){
shootRobot(power);
}else{
shootRobot(0.0f);
}
if(c==(SmartDashboard::GetNumber("First Shot Stop"))*200){ //Stop the motors when the end time is reached
shootRobot(0.0f);
currentStep=2;
c=0; //Reset the timer again
} }
//}}}
//Shoot{{{
if(currentStep==2&&c>SmartDashboard::GetNumber("First Shot Start")*200){
if(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage())){
shootRobot(power);
}else{
shootRobot(0.0f);
}
if(c==SmartDashboard::GetNumber("First Shot Stop")*200){
shootRobot(0.0f);
currentStep++;
c=0;
}
}
//}}}
//Lower Shooter{{{
if(currentStep==3){
shootRobot(0.1f);
if(c==1*200){
shootRobot(0.0f);
currentStep++;
c=0;
}
}
//}}}
//Collect Ball{{{
if(currentStep==4&&c>SmartDashboard::GetNumber("Second Shot Start")){
if(){
shootRobot(1.0f);
}
setMotorValue(6, 1, 1);
if(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage())){
}
if(c==SmartDashboard::GetNumber("Second Shot Stop")*200){
}
}
//}}}
//Shoot{{{
if(currentStep==5&&c>(SmartDashboard::GetNumber("Second Shot Start"))*200){
if(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage())){
shootRobot(power);
}else{
shootRobot(0.0f);
}
if(c==(SmartDashboard::GetNumber("First Shot Stop"))*200){
shootRobot(0.0f);
currentStep++;
c=0;
}
}
//}}}
//}}}
} }
SmartDashboard::PutNumber("Autonomous step", currentStep);
updateDashboard(); updateDashboard();
//Compressor{{{
if(i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1) { if(i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1) {
compressor.Stop(); compressor.Stop();
compressing = false; compressing = false;
@ -289,15 +508,18 @@ public:
compressing = true; compressing = true;
logMsg("Starting the compressor",2); logMsg("Starting the compressor",2);
} }
Wait(0.005f); //}}}
i++; i++;
c++; c++;
Wait(0.005f);
} }
i=0;
compressing = false; compressing = false;
compressor.Stop(); compressor.Stop();
} }
//}}}
//Teleop{{{
void OperatorControl() { void OperatorControl() {
//Initializations{{{
myRobot.SetSafetyEnabled(false); myRobot.SetSafetyEnabled(false);
int i = 0; int i = 0;
int cur=0; int cur=0;
@ -307,15 +529,9 @@ public:
compressing = false; compressing = false;
logMsg("Starting Teleop",1); logMsg("Starting Teleop",1);
SmartDashboard::PutBoolean("CollectorState",false); SmartDashboard::PutBoolean("CollectorState",false);
//}}}
while(IsEnabled() && IsOperatorControl()) { while(IsEnabled() && IsOperatorControl()) {
if(cur==50) { //Joystick{{{
cur=0;
WallLeft.Set(swap?1:0);
BallRight.Set(swap?1:0);
WallRight.Set(swap?0:1);
BallRight.Set(swap?0:1);
swap=!swap;
}
if(Lstick.GetRawButton(9)==1){ if(Lstick.GetRawButton(9)==1){
throttle = (-Lstick.GetRawAxis(4)+1)/2; throttle = (-Lstick.GetRawAxis(4)+1)/2;
}else if(Lstick.GetRawButton(10)){ }else if(Lstick.GetRawButton(10)){
@ -325,29 +541,6 @@ public:
}else if(Lstick.GetRawButton(8)){ }else if(Lstick.GetRawButton(8)){
throttle = SmartDashboard::GetNumber("ShooterButtonPower8"); throttle = SmartDashboard::GetNumber("ShooterButtonPower8");
} }
if(SmartDashboard::GetBoolean("Daniel Mode")) {
driveRobot(-Rstick.GetY(),Rstick.GetZ()+Rstick.GetX());
} else {
driveRobot(Rstick.GetY(),Rstick.GetZ()+Rstick.GetX());
}
//Log things
if(i % 200 == 0) {
//logMsg(toString(compressor.GetPressureSwitchValue()),2);
//logMsg("armPot value: "+toString(armPot.GetAverageVoltage(),11));
//logMsg("Converted armPot value: "+toString(armPot.GetAverageVoltage(),11));
}
if(SmartDashboard::GetBoolean("Compressor Enabled")){
if(i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1) {
compressor.Stop();
compressing = false;
logMsg("Stopping the compressor",2);
}
if(i % 100 == 0 && !compressing && compressor.GetPressureSwitchValue() == 0) {
compressor.Start();
compressing = true;
logMsg("Starting the compressor... again",2);
}
}
if(Lstick.GetRawButton(3)){ if(Lstick.GetRawButton(3)){
upLimit=100.0f; upLimit=100.0f;
} }
@ -360,26 +553,8 @@ public:
if(Lstick.GetRawButton(6)){ if(Lstick.GetRawButton(6)){
upLimit=130.0f; upLimit=130.0f;
} }
updateDashboard(); if(Lstick.GetRawButton(1)==1) {
if(Lstick.GetRawButton(1)==1&&Lstick.GetRawButton(2)==1){ //Shoot{{{
throttle=SmartDashboard::GetNumber("ShortRange");
shooting = true;
logMsg("Firing",13);
logMsg("Collector is extended, going to fire",17);
shootRobot(throttle);
setMotorValue(6, 1, 1);
if(collectorExtended){
shooting = true;
logMsg("Firing",13);
logMsg("Collector is extended, going to fire",17);
shootRobot(throttle);
setMotorValue(6, 1, 1);
}else{
shooting = false;
logMsg("Collector is NOT extended, not going to fire",17);
}
}else if(Lstick.GetRawButton(1)==1) {
//Move arm motors based on throttle
shooting = true; shooting = true;
logMsg("Firing",13); logMsg("Firing",13);
logMsg("Collector is extended, going to fire",17); logMsg("Collector is extended, going to fire",17);
@ -389,20 +564,16 @@ public:
shooting = false; shooting = false;
logMsg("Collector is NOT extended, not going to fire",17); logMsg("Collector is NOT extended, not going to fire",17);
} }
if(collectorExtended == true&&(upLimit>=potToDegrees(armPot.GetAverageVoltage()))) { if(collectorExtended == true&&(SmartDashboard::GetBoolean("Ignore Pot")||upLimit>=potToDegrees(armPot.GetAverageVoltage()))) {
shooting = true; shooting = true;
logMsg("Firing",13); logMsg("Firing",13);
logMsg("Collector is extended, going to fire",17); logMsg("Collector is extended, going to fire",17);
shootRobot(throttle); shootRobot(throttle);
setMotorValue(6, 1, 1); setMotorValue(6, 1, 1);
} }
} else if(Lstick.GetRawButton(1)==1&&(SmartDashboard::GetBoolean("Ignore Pot")||upLimit<=potToDegrees(armPot.GetAverageVoltage()))) { //}}}
shooting = false;
logMsg("Stopping shooter motor",13);
logMsg("Stopping collector motor",17);
shootRobot(0);
} else if(Lstick.GetRawButton(2)==1) { } else if(Lstick.GetRawButton(2)==1) {
//Reverse the arm motors //Lower Shooter{{{
shooting = false; shooting = false;
shootRobot(-0.1f); shootRobot(-0.1f);
if(collectorExtended == false) { if(collectorExtended == false) {
@ -412,22 +583,29 @@ public:
shootRobot(-0.1f); shootRobot(-0.1f);
logMsg("Collector is extended, going to fire",17); logMsg("Collector is extended, going to fire",17);
} }
//}}}
} else { } else {
//Stop Shooting{{{
shooting = false; shooting = false;
//Stop all motors
shootRobot(0); shootRobot(0);
//}}}
} }
if(Rstick.GetRawButton(9)==1) { if(Rstick.GetRawButton(9)==1) {
//Extend Collector{{{
SmartDashboard::PutBoolean("CollectorState",true); SmartDashboard::PutBoolean("CollectorState",true);
collectorExtended = true; collectorExtended = true;
collectorSole1.Set(false); collectorSole1.Set(false);
collectorSole2.Set(true); collectorSole2.Set(true);
//}}}
} else if(Rstick.GetRawButton(10)==1) { } else if(Rstick.GetRawButton(10)==1) {
//Retract Collector{{{
SmartDashboard::PutBoolean("CollectorState",false); SmartDashboard::PutBoolean("CollectorState",false);
collectorExtended = false; collectorExtended = false;
collectorSole1.Set(true); collectorSole1.Set(true);
collectorSole2.Set(false); collectorSole2.Set(false);
//}}}
} }
//Collector Motor{{{
if(Lstick.GetRawButton(11)==1) { if(Lstick.GetRawButton(11)==1) {
setMotorValue(6, 1, 1); setMotorValue(6, 1, 1);
} else if(Lstick.GetRawButton(12)==1) { } else if(Lstick.GetRawButton(12)==1) {
@ -435,10 +613,45 @@ public:
} else if(!shooting) { } else if(!shooting) {
setMotorValue(6, 1, 0); setMotorValue(6, 1, 0);
} }
//}}}
//}}}
//Driving{{{
if(Rstick.GetRawButton(1)){
if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=40.0f){
driveRobot(1.0f,SmartDashboard::GetNumber("AutoCorrection"));
}else{
driveRobot(0.0f,0.0f);
}
}else{
if(SmartDashboard::GetBoolean("Daniel Mode")) {
driveRobot(-Rstick.GetY(),Rstick.GetZ()+Rstick.GetX());
} else {
driveRobot(Rstick.GetY(),Rstick.GetZ()+Rstick.GetX());
}
}
//}}}
//Compressor{{{
if(SmartDashboard::GetBoolean("Compressor Enabled")){
if(i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1) {
compressor.Stop();
compressing = false;
logMsg("Stopping the compressor",2);
}
if(i % 100 == 0 && !compressing && compressor.GetPressureSwitchValue() == 0) {
compressor.Start();
compressing = true;
logMsg("Starting the compressor... again",2);
}
}
//}}}
updateDashboard();
cur++; cur++;
i++; i++;
Wait(0.005f); Wait(0.005f);
} }
compressing = false;
compressor.Stop();
} }
//}}}
}; };
START_ROBOT_CLASS(RobotDemo); START_ROBOT_CLASS(RobotDemo);