mirror of
https://github.com/team2059/Zaphod
synced 2025-01-07 22:14:14 -05:00
Made changes to autonomous, added a sequence changer
This commit is contained in:
parent
85d334f0fa
commit
9f14e2dbd2
451
MyRobot.cpp
451
MyRobot.cpp
@ -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)
|
||||
//Sonar in auto: drive till 40in away (dashboard value) and shoot
|
||||
//Includes{{{
|
||||
#include "WPILib.h"
|
||||
#include "SmartDashboard/SmartDashboard.h"
|
||||
#include <iostream>
|
||||
#include <math.h>
|
||||
#include <vector>
|
||||
#include <sstream>
|
||||
//}}}
|
||||
class RobotDemo : public SimpleRobot
|
||||
{
|
||||
//Declarations{{{
|
||||
RobotDrive myRobot;
|
||||
float potVal, multiplier, throttle;
|
||||
bool collectorExtended, shooting, compressing, allowCompressing;
|
||||
@ -19,8 +22,10 @@ class RobotDemo : public SimpleRobot
|
||||
AnalogChannel armPot;
|
||||
AnalogChannel BallSonicLeft, BallSonicRight, WallSonicLeft, WallSonicRight;
|
||||
DigitalOutput BallLeft, BallRight, WallLeft, WallRight;
|
||||
//}}}
|
||||
public:
|
||||
RobotDemo():
|
||||
//Initializations{{{
|
||||
//Joysticks
|
||||
Rstick(1),
|
||||
Lstick(2),
|
||||
@ -57,6 +62,8 @@ public:
|
||||
myRobot(Left1, Left2, Right1, Right2) {
|
||||
GetWatchdog().SetEnabled(false);
|
||||
}
|
||||
//}}}
|
||||
//RobotInit{{{
|
||||
void RobotInit() {
|
||||
DashboardSetup();
|
||||
multiplier = 1.0f;
|
||||
@ -67,6 +74,8 @@ public:
|
||||
allowCompressing = true;
|
||||
throttle=0;
|
||||
}
|
||||
//}}}
|
||||
//DashboardSetup{{{
|
||||
void DashboardSetup() {
|
||||
SmartDashboard::PutNumber("Throttle", throttle);
|
||||
SmartDashboard::PutNumber("upLimit", 120.0f);
|
||||
@ -81,11 +90,16 @@ public:
|
||||
SmartDashboard::PutNumber("AutoPower",0.455f);
|
||||
SmartDashboard::PutNumber("AutoCorrection",0.06f);
|
||||
SmartDashboard::PutNumber("Inital Drive Timeout", 2.5f);
|
||||
SmartDashboard::PutNumber("First Shot Start", 2.5f);
|
||||
SmartDashboard::PutNumber("First Shot Stop", 0.5f);
|
||||
SmartDashboard::PutNumber("Reverse direction start",3.5.0f);
|
||||
SmartDashboard::PutNumber("Reverse direction stop",5.5.0f);
|
||||
SmartDashboard::PutNumber("First Shot Start", 0.5f);
|
||||
SmartDashboard::PutNumber("First Shot Stop", 1.0f);
|
||||
SmartDashboard::PutNumber("Reverse direction start",0.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 sequence", 0.0f);
|
||||
//Shooter presets
|
||||
SmartDashboard::PutNumber("ShortRange",0.465f); //Power for the shooter when against the low goal
|
||||
SmartDashboard::PutNumber("ShooterButtonPower10",0.605f);
|
||||
@ -101,6 +115,8 @@ public:
|
||||
SmartDashboard::PutBoolean("Ignore Pot",false);
|
||||
//Battery voltage
|
||||
}
|
||||
//}}}
|
||||
//updateDashboard{{{
|
||||
void updateDashboard() {
|
||||
SmartDashboard::PutNumber("Throttle", throttle);
|
||||
SmartDashboard::PutNumber("armPot", potToDegrees(armPot.GetAverageVoltage()));
|
||||
@ -115,42 +131,52 @@ public:
|
||||
upLimit = 167;
|
||||
}
|
||||
}
|
||||
void shootRobot(float power=0) {
|
||||
setMotorValue(4, 1, cvt(power));
|
||||
setMotorValue(5, 1, cvt(power));
|
||||
setMotorValue(4, 2, cvt(-power));
|
||||
setMotorValue(5, 2, cvt(-power));
|
||||
}
|
||||
//}}}
|
||||
//shootRobot{{{
|
||||
void shootRobot(float power=0) {
|
||||
setMotorValue(4, 1, 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) {
|
||||
if((int)SmartDashboard::GetNumber("Log Level") % level == 0) {
|
||||
printf((message+"\n").c_str());
|
||||
}
|
||||
}
|
||||
void driveRobot(float x, float y) {
|
||||
if(y>1.0f) {
|
||||
y=1.0f;
|
||||
} else if(y!=0.0f&&y<-1.0f) {
|
||||
y=-1.0f;
|
||||
//}}}
|
||||
//toString{{{
|
||||
template<typename numbertype> string toString(numbertype a) {
|
||||
stringstream ss;
|
||||
ss<<a;
|
||||
string s = ss.str();
|
||||
return s;
|
||||
}
|
||||
int leftPower = ((y+x)/2+1)*127+1;
|
||||
int rightPower = ((y-x)/2+1)*127+1;
|
||||
//logMsg("leftPower: "+toString<int>(leftPower),3);
|
||||
//logMsg("rightPower: "+toString<int>(rightPower),3);
|
||||
//logMsg("JoyX: "+toString<float>(Rstick.GetX()),3);
|
||||
//logMsg("JoyY: "+toString<float>(Rstick.GetY()),3);
|
||||
setMotorValue(1, 1, leftPower);
|
||||
setMotorValue(2, 1, leftPower);
|
||||
setMotorValue(3, 1, leftPower);
|
||||
setMotorValue(1, 2, rightPower);
|
||||
setMotorValue(2, 2, rightPower);
|
||||
setMotorValue(3, 2, rightPower);
|
||||
}
|
||||
template<typename numbertype> string toString(numbertype a) {
|
||||
stringstream ss;
|
||||
ss<<a;
|
||||
string s = ss.str();
|
||||
return s;
|
||||
}
|
||||
//}}}
|
||||
//driveRobot{{{
|
||||
void driveRobot(float x, float y) {
|
||||
if(y>1.0f) {
|
||||
y=1.0f;
|
||||
} else if(y!=0.0f&&y<-1.0f) {
|
||||
y=-1.0f;
|
||||
}
|
||||
int leftPower = ((y+x)/2+1)*127+1;
|
||||
int rightPower = ((y-x)/2+1)*127+1;
|
||||
//logMsg("leftPower: "+toString<int>(leftPower),3);
|
||||
//logMsg("rightPower: "+toString<int>(rightPower),3);
|
||||
//logMsg("JoyX: "+toString<float>(Rstick.GetX()),3);
|
||||
//logMsg("JoyY: "+toString<float>(Rstick.GetY()),3);
|
||||
setMotorValue(1, 1, leftPower);
|
||||
setMotorValue(2, 1, leftPower);
|
||||
setMotorValue(3, 1, leftPower);
|
||||
setMotorValue(1, 2, rightPower);
|
||||
setMotorValue(2, 2, rightPower);
|
||||
setMotorValue(3, 2, rightPower);
|
||||
}
|
||||
//}}}
|
||||
//voltToDistance{{{
|
||||
float voltToDistance(float a,bool wall=false) {
|
||||
if(wall) {
|
||||
return (a / 0.00488f) / 2.54f;
|
||||
@ -158,6 +184,8 @@ public:
|
||||
return (a / 0.000976562f) / 25.4f;
|
||||
}
|
||||
}
|
||||
//}}}
|
||||
//potToDegrees{{{
|
||||
float potToDegrees(float a) {
|
||||
float max = -.0003948;
|
||||
float min = 5.0245547;
|
||||
@ -166,13 +194,17 @@ public:
|
||||
max = max - max; //=0
|
||||
return 300 - ((b + max) * (300 / min));
|
||||
}
|
||||
//}}}
|
||||
//cvt{{{
|
||||
int cvt(float input) {
|
||||
return input * 127.0f + 128;
|
||||
}
|
||||
//}}}
|
||||
//setMotorValue{{{
|
||||
void setMotorValue(int motor, int subwayStation = 1, int value = 127) {
|
||||
if(subwayStation == 1) {
|
||||
//subwayStation1{{{
|
||||
switch(motor) {
|
||||
//Drive motors
|
||||
case 1:
|
||||
Left1.SetRaw(value);
|
||||
break;
|
||||
@ -200,7 +232,9 @@ public:
|
||||
case 10:
|
||||
break;
|
||||
}
|
||||
//}}}
|
||||
} else if(subwayStation == 2) {
|
||||
//subwayStation2{{{
|
||||
switch(motor) {
|
||||
//Shooter motors
|
||||
case 1:
|
||||
@ -229,15 +263,20 @@ public:
|
||||
case 10:
|
||||
break;
|
||||
}
|
||||
//}}}
|
||||
}
|
||||
}
|
||||
//}}}
|
||||
//Test{{{
|
||||
void Test() {
|
||||
}
|
||||
//}}}
|
||||
//Autonomous{{{
|
||||
void Autonomous() {
|
||||
//Initializations{{{
|
||||
myRobot.SetSafetyEnabled(false);
|
||||
int i=0;
|
||||
int c=0;
|
||||
//TODO delete this and implement it
|
||||
float getSecondBallStart=(SmartDashboard::GetNumber("Reverse direction start"))*200;
|
||||
float getSecondBallStop=(SmartDashboard::GetNumber("Reverse direction stop"))*200;
|
||||
float power=SmartDashboard::GetNumber("AutoPower");
|
||||
@ -251,34 +290,214 @@ public:
|
||||
WallRight.Set(1);
|
||||
BallRight.Set(0);
|
||||
SmartDashboard::PutBoolean("CollectorState",true);
|
||||
//}}}
|
||||
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)
|
||||
//Displays the step that is currently running in Autonomous
|
||||
SmartDashboard::PutNumber("Autonomous step", currentStep);
|
||||
if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=40.0f){ //Particular only to the first drive step
|
||||
driveRobot(1.0f,correction);
|
||||
if(SmartDashboard::GetNumber("Autonomous Sequence")==0){
|
||||
//Autonomous0{{{
|
||||
//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;
|
||||
}
|
||||
}
|
||||
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);
|
||||
currentStep=1;
|
||||
c=0; //Reset the timer
|
||||
}
|
||||
}
|
||||
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
|
||||
if(c==50){
|
||||
currentStep++;
|
||||
c=0;
|
||||
}
|
||||
}
|
||||
//}}}
|
||||
//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();
|
||||
//Compressor{{{
|
||||
if(i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1) {
|
||||
compressor.Stop();
|
||||
compressing = false;
|
||||
@ -289,15 +508,18 @@ public:
|
||||
compressing = true;
|
||||
logMsg("Starting the compressor",2);
|
||||
}
|
||||
Wait(0.005f);
|
||||
//}}}
|
||||
i++;
|
||||
c++;
|
||||
Wait(0.005f);
|
||||
}
|
||||
i=0;
|
||||
compressing = false;
|
||||
compressor.Stop();
|
||||
}
|
||||
//}}}
|
||||
//Teleop{{{
|
||||
void OperatorControl() {
|
||||
//Initializations{{{
|
||||
myRobot.SetSafetyEnabled(false);
|
||||
int i = 0;
|
||||
int cur=0;
|
||||
@ -307,15 +529,9 @@ public:
|
||||
compressing = false;
|
||||
logMsg("Starting Teleop",1);
|
||||
SmartDashboard::PutBoolean("CollectorState",false);
|
||||
//}}}
|
||||
while(IsEnabled() && IsOperatorControl()) {
|
||||
if(cur==50) {
|
||||
cur=0;
|
||||
WallLeft.Set(swap?1:0);
|
||||
BallRight.Set(swap?1:0);
|
||||
WallRight.Set(swap?0:1);
|
||||
BallRight.Set(swap?0:1);
|
||||
swap=!swap;
|
||||
}
|
||||
//Joystick{{{
|
||||
if(Lstick.GetRawButton(9)==1){
|
||||
throttle = (-Lstick.GetRawAxis(4)+1)/2;
|
||||
}else if(Lstick.GetRawButton(10)){
|
||||
@ -325,29 +541,6 @@ public:
|
||||
}else if(Lstick.GetRawButton(8)){
|
||||
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)){
|
||||
upLimit=100.0f;
|
||||
}
|
||||
@ -360,26 +553,8 @@ public:
|
||||
if(Lstick.GetRawButton(6)){
|
||||
upLimit=130.0f;
|
||||
}
|
||||
updateDashboard();
|
||||
if(Lstick.GetRawButton(1)==1&&Lstick.GetRawButton(2)==1){
|
||||
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
|
||||
if(Lstick.GetRawButton(1)==1) {
|
||||
//Shoot{{{
|
||||
shooting = true;
|
||||
logMsg("Firing",13);
|
||||
logMsg("Collector is extended, going to fire",17);
|
||||
@ -389,20 +564,16 @@ public:
|
||||
shooting = false;
|
||||
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;
|
||||
logMsg("Firing",13);
|
||||
logMsg("Collector is extended, going to fire",17);
|
||||
shootRobot(throttle);
|
||||
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) {
|
||||
//Reverse the arm motors
|
||||
//Lower Shooter{{{
|
||||
shooting = false;
|
||||
shootRobot(-0.1f);
|
||||
if(collectorExtended == false) {
|
||||
@ -412,22 +583,29 @@ public:
|
||||
shootRobot(-0.1f);
|
||||
logMsg("Collector is extended, going to fire",17);
|
||||
}
|
||||
//}}}
|
||||
} else {
|
||||
//Stop Shooting{{{
|
||||
shooting = false;
|
||||
//Stop all motors
|
||||
shootRobot(0);
|
||||
//}}}
|
||||
}
|
||||
if(Rstick.GetRawButton(9)==1) {
|
||||
//Extend Collector{{{
|
||||
SmartDashboard::PutBoolean("CollectorState",true);
|
||||
collectorExtended = true;
|
||||
collectorSole1.Set(false);
|
||||
collectorSole2.Set(true);
|
||||
//}}}
|
||||
} else if(Rstick.GetRawButton(10)==1) {
|
||||
//Retract Collector{{{
|
||||
SmartDashboard::PutBoolean("CollectorState",false);
|
||||
collectorExtended = false;
|
||||
collectorSole1.Set(true);
|
||||
collectorSole2.Set(false);
|
||||
//}}}
|
||||
}
|
||||
//Collector Motor{{{
|
||||
if(Lstick.GetRawButton(11)==1) {
|
||||
setMotorValue(6, 1, 1);
|
||||
} else if(Lstick.GetRawButton(12)==1) {
|
||||
@ -435,10 +613,45 @@ public:
|
||||
} else if(!shooting) {
|
||||
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++;
|
||||
i++;
|
||||
Wait(0.005f);
|
||||
}
|
||||
compressing = false;
|
||||
compressor.Stop();
|
||||
}
|
||||
//}}}
|
||||
};
|
||||
START_ROBOT_CLASS(RobotDemo);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user