team2059/zaphod
2
0
Fork 0
mirror of https://github.com/team2059/Zaphod synced 2024-12-18 20:12:28 -05:00
Code Issues Releases Wiki Activity

Added an alternative method for autonomus

Browse Source
This commit is contained in:
Adam Long 2014-03-06 23:44:23 +00:00
parent 500575d838
commit 00b7d80479
1 changed files with 153 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

158
MyRobot.cpp
View File

@ -3,7 +3,6 @@
//Sonar in auto: drive till 40in away (dashboard value) and shoot //Sonar in auto: drive till 40in away (dashboard value) and shoot
#include "WPILib.h" #include "WPILib.h"
#include "SmartDashboard/SmartDashboard.h" #include "SmartDashboard/SmartDashboard.h"
//#include "Command.h"
#include <iostream> #include <iostream>
#include <math.h> #include <math.h>
#include <vector> #include <vector>
@ -21,7 +20,6 @@ class RobotDemo : public SimpleRobot
Compressor compressor; Compressor compressor;
Jaguar Left1, Left2, Left3, Right1, Right2, Right3, RightArmMotor1, RightArmMotor2, LeftArmMotor1, LeftArmMotor2, CollectorMotor1; Jaguar Left1, Left2, Left3, Right1, Right2, Right3, RightArmMotor1, RightArmMotor2, LeftArmMotor1, LeftArmMotor2, CollectorMotor1;
AnalogChannel armPot; AnalogChannel armPot;
//Ultrasonic
AnalogChannel BallSonicLeft, BallSonicRight, WallSonicLeft, WallSonicRight; AnalogChannel BallSonicLeft, BallSonicRight, WallSonicLeft, WallSonicRight;
DigitalOutput BallLeft, BallRight, WallLeft, WallRight; DigitalOutput BallLeft, BallRight, WallLeft, WallRight;
public: public:
@ -100,6 +98,9 @@ public:
SmartDashboard::PutBoolean("Use Ultrasonic",false); SmartDashboard::PutBoolean("Use Ultrasonic",false);
SmartDashboard::PutBoolean("Daniel Mode",false); SmartDashboard::PutBoolean("Daniel Mode",false);
SmartDashboard::PutBoolean("CollectorState",false); SmartDashboard::PutBoolean("CollectorState",false);
SmartDashboard::PutBoolean("Compressor Enabled", allowCompressing);
SmartDashboard::PutBoolean("Compressor Running", compressing);
//Battery voltage
} }
void updateDashboard() { void updateDashboard() {
SmartDashboard::PutNumber("Throttle", throttle); SmartDashboard::PutNumber("Throttle", throttle);
@ -109,6 +110,8 @@ public:
SmartDashboard::PutNumber("Ball Left", voltToDistance(BallSonicLeft.GetAverageVoltage())); SmartDashboard::PutNumber("Ball Left", voltToDistance(BallSonicLeft.GetAverageVoltage()));
SmartDashboard::PutNumber("Ball Right", voltToDistance(BallSonicRight.GetAverageVoltage())); SmartDashboard::PutNumber("Ball Right", voltToDistance(BallSonicRight.GetAverageVoltage()));
SmartDashboard::PutNumber("upLimit", upLimit); SmartDashboard::PutNumber("upLimit", upLimit);
SmartDashboard::PutBoolean("Compressor Running", compressing);
allowCompressing = SmartDashboard::GetBoolean("Compressor Enabled");
if(upLimit > 167) { if(upLimit > 167) {
upLimit = 167; upLimit = 167;
} }
@ -144,7 +147,6 @@ public:
setMotorValue(3, 2, rightPower); setMotorValue(3, 2, rightPower);
} }
template<typename numbertype> string toString(numbertype a) { template<typename numbertype> string toString(numbertype a) {
//TODO
stringstream ss; stringstream ss;
ss<<a; ss<<a;
string s = ss.str(); string s = ss.str();
@ -236,6 +238,7 @@ public:
myRobot.SetSafetyEnabled(false); myRobot.SetSafetyEnabled(false);
int avgDist; int avgDist;
int commandIndex=0; int commandIndex=0;
int averageAmount=5;
int i=0; int i=0;
int cur=0; int cur=0;
int averageAmount=5; int averageAmount=5;
@ -294,9 +297,10 @@ public:
if(i<1700+3*x+2*y){ if(i<1700+3*x+2*y){
setMotorValue(6, 1, 1); setMotorValue(6, 1, 1);
} }
if(i<200+x) { }
//Forward .5s if(i-c==1&&c>startShootingFirst){ //The next 'step' in auto, shooting the motors when startShootingFirst is true
driveRobot(-1.0f,correction); shootRobot(power);
if(c==stopShootingFirst){ //Stop the motors when the end time is reached
shootRobot(0.0f); shootRobot(0.0f);
}else if(i>=200+x&&i<=400+x){ }else if(i>=200+x&&i<=400+x){
//Wait //Wait
@ -347,6 +351,98 @@ public:
shootRobot(0.0f); shootRobot(0.0f);
} }
} }
// if(i-c==2&&c>getSecondBallStart){
//
//}
// **Start of time based auto
// if(SmartDashboard::GetBoolean("OneBallAuto")){
// setMotorValue(6, 1, 1);
// if(i<200+x) {
// //Forward .5s
// driveRobot(-1.0f,correction);
// shootRobot(0.0f);
// }else if(i>=200+x&&i<=400+x){
// //Wait
// driveRobot(0.0f, 0.0f);
// shootRobot(0.0f);
// } else if(i>400+x&&i<500+x&&angle>=potToDegrees(armPot.GetAverageVoltage())) {
// //Shoot
// driveRobot(0.0f, 0.0f);
// shootRobot(power);
// } else if(i>400+x&&i<500+x&&angle<=potToDegrees(armPot.GetAverageVoltage())) {
// //Wait
// driveRobot(0.0f, 0.0f);
// shootRobot(0.0f);
// } else if(i>=500) {
// //Kill robit
// driveRobot(0.0f, 0.0f);
// shootRobot(0.0f);
// }
// }else if(SmartDashboard::GetBoolean("Use Ultrasonic")){
// if(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)>=40.0f){
// driveRobot(1.0f,correction);
// i=0;
// }else{
// driveRobot(0.0f, 0.0f);
// }
// else if()
// }else{
// if(i<1700+3*x+2*y){
// setMotorValue(6, 1, 1);
// }
// if(i<200+x) {
// //Forward .5s
// driveRobot(-1.0f,correction);
// shootRobot(0.0f);
// }else if(i>=200+x&&i<=400+x){
// //Wait
// driveRobot(0.0f, 0.0f);
// shootRobot(0.0f);
// } else if(i>400+x&&i<500+x&&angle>=potToDegrees(armPot.GetAverageVoltage())) {
// //Shoot
// driveRobot(0.0f, 0.0f);
// shootRobot(power);
// } else if(i>400+x&&i<500+x&&angle<=potToDegrees(armPot.GetAverageVoltage())) {
// //Wait
// driveRobot(0.0f, 0.0f);
// shootRobot(0.0f);
// } else if(i>500+x&&i<700+2*x+y) {
// //Drive backward 1s, Collect ball
// if(40.0f<=potToDegrees(armPot.GetAverageVoltage())){
// shootRobot(-0.30f);
// }
// driveRobot(0.6f,correction);
// shootRobot(0.0f);
// } else if(i>=700+2*x+y&&i<=1300+2*x+y){
// //Wait
// driveRobot(0.0f,0.0f);
// shootRobot(0.0f);
// } else if(i>1300+2*x+y&&i<1500+3*x+2*y) {
// //Drive forward 1s
// driveRobot(-1.0f,correction);
// shootRobot(0.0f);
// } else if(i>=1500+3*x+2*y&&i<=1600+3*x+2*y){
// //Wait
// driveRobot(0.0f,0.0f);
// shootRobot(0.0f);
// } else if(i>1600+3*x+2*y&&i<1700+3*x+2*y&&/*120*/angle>=potToDegrees(armPot.GetAverageVoltage())){
// //Shoot
// driveRobot(0.0f,0.0f);
// shootRobot(power);
// } else if(i>1600+3*x+2*y&&i<1700+3*x+2*y&&/*120*/angle<=potToDegrees(armPot.GetAverageVoltage())){
// //Wait
// driveRobot(0.0f,0.0f);
// shootRobot(0.0f);
// } else if(i>1700+3*x+2*y&&40.0f<=potToDegrees(armPot.GetAverageVoltage())) {
// //Stop robot after auto, let down shooter
// driveRobot(0.0f,0.0f);
// shootRobot(-0.15);
// } else if(i>1700+3*x+2*y&&40.0f>=potToDegrees(armPot.GetAverageVoltage())) {
// //Stop all motors
// driveRobot(0.0f,0.0f);
// shootRobot(0.0f);
// }
// }
updateDashboard(); updateDashboard();
if(i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1) { if(i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1) {
compressor.Stop(); compressor.Stop();
@ -360,7 +456,7 @@ public:
} }
Wait(0.005f); Wait(0.005f);
i++; i++;
cur++; c++;
} }
i=0; i=0;
compressing = false; compressing = false;
@ -405,6 +501,7 @@ public:
//logMsg("armPot value: "+toString(armPot.GetAverageVoltage(),11)); //logMsg("armPot value: "+toString(armPot.GetAverageVoltage(),11));
//logMsg("Converted 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) { if(i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1) {
compressor.Stop(); compressor.Stop();
compressing = false; compressing = false;
@ -415,6 +512,7 @@ public:
compressing = true; compressing = true;
logMsg("Starting the compressor... again",2); logMsg("Starting the compressor... again",2);
} }
}
if(Lstick.GetRawButton(3)){ if(Lstick.GetRawButton(3)){
upLimit=100.0f; upLimit=100.0f;
} }
@ -427,33 +525,42 @@ public:
if(Lstick.GetRawButton(6)){ if(Lstick.GetRawButton(6)){
upLimit=130.0f; upLimit=130.0f;
} }
//TODO
updateDashboard(); updateDashboard();
if(Lstick.GetRawButton(1)==1&&Lstick.GetRawButton(2)==1){ if(Lstick.GetRawButton(1)==1&&Lstick.GetRawButton(2)==1){
throttle=SmartDashboard::GetNumber("ShortRange"); throttle=SmartDashboard::GetNumber("ShortRange");
if(collectorExtended){
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(collectorExtended){
shooting = false; // shooting = true;
logMsg("Collector is NOT extended, not going to fire",17); // 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) { }else if(Lstick.GetRawButton(1)==1) {
//Move arm motors based on throttle //Move arm motors based on throttle
if(collectorExtended == false) {
shooting = false;
logMsg("Collector is NOT extended, not going to fire",17);
}
if(collectorExtended == true&&(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);
} // if(collectorExtended == false) {
// shooting = false;
// logMsg("Collector is NOT extended, not going to fire",17);
// }
// if(collectorExtended == true&&(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&&(upLimit<=potToDegrees(armPot.GetAverageVoltage()))) { } else if(Lstick.GetRawButton(1)==1&&(upLimit<=potToDegrees(armPot.GetAverageVoltage()))) {
shooting = false; shooting = false;
logMsg("Stopping shooter motor",13); logMsg("Stopping shooter motor",13);
@ -462,13 +569,14 @@ public:
} else if(Lstick.GetRawButton(2)==1) { } else if(Lstick.GetRawButton(2)==1) {
//Reverse the arm motors //Reverse the arm motors
shooting = false; shooting = false;
if(collectorExtended == false) {
logMsg("Collector is not extended, not going to fire",17);
}
if(collectorExtended == true) {
shootRobot(-0.1f); shootRobot(-0.1f);
logMsg("Collector is extended, going to fire",17); // if(collectorExtended == false) {
} // logMsg("Collector is not extended, not going to fire",17);
// }
// if(collectorExtended == true) {
// shootRobot(-0.1f);
// logMsg("Collector is extended, going to fire",17);
// }
} else { } else {
shooting = false; shooting = false;
//Stop all motors //Stop all motors