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

Committing before driving voyage

Browse Source
This commit is contained in:
Austen Adler 2014-02-26 20:50:12 +00:00
parent 034d3450f1
commit 938a8b74f8
1 changed files with 139 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

216
MyRobot.cpp
View File

@ -1,9 +1,8 @@
//TODO: //TODO this
//Auto
//Sonar code //Sonar code
#include "WPILib.h" #include "WPILib.h"
#include "SmartDashboard/SmartDashboard.h" #include "SmartDashboard/SmartDashboard.h"
#include "Command.h" //#include "Command.h"
#include <iostream> #include <iostream>
#include <math.h> #include <math.h>
#include <vector> #include <vector>
@ -14,10 +13,9 @@ class RobotDemo : public SimpleRobot
float potVal, multiplier, servoXState, servoYState, throttle, ServoXJoyPos, ServoYJoyPos; float potVal, multiplier, servoXState, servoYState, throttle, ServoXJoyPos, ServoYJoyPos;
int lastToggle; int lastToggle;
int collectorSpeed; int collectorSpeed;
bool collectorExtended, toggleCollector; bool collectorExtended, toggleCollector, shooting, compressing;
bool shooting, compressing;
float DownSpeed, downLimit, upLimit; float DownSpeed, downLimit, upLimit;
string cmd; //string cmd;
Joystick Rstick, Lstick; Joystick Rstick, Lstick;
Servo Servo1, Servo2; Servo Servo1, Servo2;
Solenoid collectorSole1, collectorSole2; Solenoid collectorSole1, collectorSole2;
@ -45,7 +43,6 @@ public:
WallLeft(1,5), WallLeft(1,5),
BallRight(2,4), BallRight(2,4),
WallRight(2,5), WallRight(2,5),
//Compressor //Compressor
//compressor(2,3,1,1), //compressor(2,3,1,1),
compressor(2, 5, 1, 1), //Solenoids compressor(2, 5, 1, 1), //Solenoids
@ -98,7 +95,7 @@ public:
SmartDashboard::PutNumber("downLimit", downLimit); SmartDashboard::PutNumber("downLimit", downLimit);
SmartDashboard::PutNumber("upLimit", upLimit); SmartDashboard::PutNumber("upLimit", upLimit);
SmartDashboard::PutNumber("DownSpeed", 0.100); SmartDashboard::PutNumber("DownSpeed", 0.100);
SmartDashboard::PutString("Auto", cmd); //SmartDashboard::PutString("Auto", cmd);
SmartDashboard::PutNumber("collectorSpeed", 127); SmartDashboard::PutNumber("collectorSpeed", 127);
SmartDashboard::PutNumber("armPot", potToDegrees(armPot.GetAverageVoltage())); SmartDashboard::PutNumber("armPot", potToDegrees(armPot.GetAverageVoltage()));
SmartDashboard::PutNumber("Log Level", 1); SmartDashboard::PutNumber("Log Level", 1);
@ -107,13 +104,13 @@ public:
SmartDashboard::PutNumber("Wall Right", voltToDistance(WallSonicRight.GetAverageVoltage(),true)); SmartDashboard::PutNumber("Wall Right", voltToDistance(WallSonicRight.GetAverageVoltage(),true));
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::PutBoolean("Use ultrasonic",false); SmartDashboard::PutBoolean("Use Ultrasonic",false);
SmartDashboard::PutBoolean("Reverse Robot",false);
} }
void updateDashboard() { void updateDashboard() {
SmartDashboard::PutNumber("Throttle", throttle); SmartDashboard::PutNumber("Throttle", throttle);
collectorSpeed = SmartDashboard::GetNumber("collectorSpeed"); collectorSpeed = SmartDashboard::GetNumber("collectorSpeed");
SmartDashboard::PutNumber("armPot", potToDegrees(armPot.GetAverageVoltage())); SmartDashboard::PutNumber("armPot", potToDegrees(armPot.GetAverageVoltage()));
//Ultrasonic
SmartDashboard::PutNumber("Wall Left", voltToDistance(WallSonicLeft.GetAverageVoltage(),true)); SmartDashboard::PutNumber("Wall Left", voltToDistance(WallSonicLeft.GetAverageVoltage(),true));
SmartDashboard::PutNumber("Wall Right", voltToDistance(WallSonicRight.GetAverageVoltage(),true)); SmartDashboard::PutNumber("Wall Right", voltToDistance(WallSonicRight.GetAverageVoltage(),true));
SmartDashboard::PutNumber("Ball Left", voltToDistance(BallSonicLeft.GetAverageVoltage())); SmartDashboard::PutNumber("Ball Left", voltToDistance(BallSonicLeft.GetAverageVoltage()));
@ -129,9 +126,7 @@ public:
} }
} }
void shootRobot(float power=0, bool override=false) { void shootRobot(float power=0, bool override=false) {
if(!SmartDashboard::GetBoolean("Use ultrasonic")){ override=true;
override=true;
}
//Needs a limit to help the driver aim //Needs a limit to help the driver aim
//In this case its checking that we are no more than 15 degrees off //In this case its checking that we are no more than 15 degrees off
//The override is in place in case an ultrasonic becomes damaged and we are unable to validate the distance through software //The override is in place in case an ultrasonic becomes damaged and we are unable to validate the distance through software
@ -280,78 +275,67 @@ public:
} }
void Autonomous() { void Autonomous() {
myRobot.SetSafetyEnabled(false); myRobot.SetSafetyEnabled(false);
cmd = SmartDashboard::GetString("Auto"); //cmd = SmartDashboard::GetString("Auto");
int avgDist;
int commandIndex=0; int commandIndex=0;
int i=0; int i=0;
int cur=0; int cur=0;
int averageAmount=5; int averageAmount=5;
compressing=false; float initalDriveTime=1.4; //The amount of time in seconds that we will drive forward at the start of the match
collectorSole1.Set(false); float shooterMaxAngle=125; //The maximum angle that the arm can shoot to during all of auto
collectorSole2.Set(true); float shooterDelay = 4; //The amount of time in seconds between the inital drive time and the shooter firing
int avgDist; float shooterDuration = .5;//The amount of time in seconds that the shooter motors will be moving
shooterDelay = (initalDriveTime*200)+(shooterDelay*4); //Do math to figure out the times to start the shooting
float sampleCount=12;
float avgLeft=0; float avgLeft=0;
float avgRight=0; float avgRight=0;
float curDist; float curDist;
WallLeft.Set(1);
BallLeft.Set(1);
WallRight.Set(0);
BallRight.Set(0);
float thisIsATest; float thisIsATest;
while (IsEnabled() && IsAutonomous()) { compressing=false;
//Collect left average values from cur values 0 to 12 collectorSole1.Set(false);
if(cur<12){ collectorSole2.Set(true);
WallLeft.Set(1); WallLeft.Set(1);
BallLeft.Set(1); BallLeft.Set(0);
WallRight.Set(0); WallRight.Set(1);
BallRight.Set(0); BallRight.Set(0);
avgLeft+=voltToDistance(WallSonicLeft.GetAverageVoltage()); while(IsEnabled()&&IsAutonomous()){
//After 12 cur iterations, find the average (data was recorded 12 times, divide by 12) if(SmartDashboard::GetBoolean("Use Ultrasonic")){
}else if(cur==12){ if(cur<sampleCount) {
avgLeft/=12; WallLeft.Set(1);
//Collect right average values from cur values 38 to 50 WallRight.Set(0);
}else if(cur<50&&cur>38){ avgLeft+=voltToDistance(WallSonicLeft.GetAverageVoltage(),true);
WallLeft.Set(0); }else if(cur==sampleCount){
BallLeft.Set(0); avgLeft/=12;
WallRight.Set(1); }else if(cur<sampleCount*4&&cur>3*sampleCount){
BallRight.Set(1); WallLeft.Set(0);
avgRight+=voltToDistance(WallSonicRight.GetAverageVoltage()); WallRight.Set(1);
//After 50 cur iterations, find the average (data was recorded 12 times, divide by 12) avgRight+=voltToDistance(WallSonicRight.GetAverageVoltage(),true);
}else if(cur==50){ }else if(cur==sampleCount*4){
cur=0; avgRight/=12;
avgRight/=12; thisIsATest=atan((abs(avgLeft-avgRight))/20.0f)*360.0f/(2.0f*3.141592653589793f);
thisIsATest=atan((abs(avgLeft-avgRight))/20.0f)*360.0f/(2.0f*3.141592653589793f); }else if(cur==sampleCount*6){
SmartDashboard::PutNumber("Tanval", atan((abs(avgLeft-avgRight))/20.0f)*360.0f/(2.0f*3.141592653589793f)); cur=0;
} }
//Calculate the inital distance and average it averageAmount times. if(i>164){
if(i<averageAmount){ SmartDashboard::PutNumber("Tanval", thisIsATest);
avgDist+=(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)+voltToDistance(WallSonicRight.GetAverageVoltage(),true)/2); }
}else{ }else{
avgDist/=averageAmount; if (i<=initalDriveTime*200) {
} setMotorValue(6, 1, 1);
//Calculate the average distance from the wall } else if (i>shooterDelay&&i<shooterDelay+(shooterDuration*200)&&shooterMaxAngle>=potToDegrees(armPot.GetAverageVoltage())) {
curDist=thisIsATest; driveRobot(0, 0);
if(i%100==0){ shootRobot(1, true);
printf("Difference: %f\n",avgDist-curDist); setMotorValue(6, 1, 1);
} } else if (i>1500&&i<1700) {
if (i>=5&&avgDist-curDist<=36.0f) { shootRobot(.1,true);
float xPower, yPower; //driveRobot(-1,0);
xPower=1; } else {
yPower=(avgDist-curDist)/36.0f; /*
if(yPower>1){ driveRobot(0, 0);
yPower=1; shootRobot(0, true);
setMotorValue(6, 1, 0);
*/
} }
driveRobot(yPower, xPower);
setMotorValue(6, 1, 1);
} else if (i>1400&&i<1600&&125>=potToDegrees(armPot.GetAverageVoltage())) {
driveRobot(0, 0);
shootRobot(1, false);
setMotorValue(6, 1, 1);
} else if (i>1500&&i<1700) {
driveRobot(-1,0);
} else {
driveRobot(0, 0);
shootRobot(0, true);
setMotorValue(6, 1, 0);
} }
updateDashboard(); updateDashboard();
if (i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1) { if (i % 100 == 0 && compressing && compressor.GetPressureSwitchValue() == 1) {
@ -370,6 +354,80 @@ public:
} }
compressing = false; compressing = false;
compressor.Stop(); compressor.Stop();
// while (IsEnabled() && IsAutonomous()) {
// //Collect left average values from cur values 0 to 12
// if(cur<12){
// WallLeft.Set(1);
// BallLeft.Set(1);
// WallRight.Set(0);
// BallRight.Set(0);
// avgLeft+=voltToDistance(WallSonicLeft.GetAverageVoltage(),true);
// //After 12 cur iterations, find the average (data was recorded 12 times, divide by 12)
// }else if(cur==12){
// avgLeft/=12;
// //Collect right average values from cur values 38 to 50
// }else if(cur<100&&cur>88){
// WallLeft.Set(0);
// BallLeft.Set(0);
// WallRight.Set(1);
// BallRight.Set(1);
// avgRight+=voltToDistance(WallSonicRight.GetAverageVoltage(),true);
// //After 50 cur iterations, find the average (data was recorded 12 times, divide by 12)
// }else if(cur==100){
// cur=0;
// avgRight/=12;
// thisIsATest=atan((abs(avgLeft-avgRight))/20.0f)*360.0f/(2.0f*3.141592653589793f);
// SmartDashboard::PutNumber("Tanval", atan((abs(avgLeft-avgRight))/20.0f)*360.0f/(2.0f*3.141592653589793f));
// }
// //Calculate the inital distance and average it averageAmount times.
// if(i<averageAmount){
// avgDist+=(voltToDistance(WallSonicLeft.GetAverageVoltage(),true)+voltToDistance(WallSonicRight.GetAverageVoltage(),true)/2);
// }else{
// avgDist/=averageAmount;
// }
// //Calculate the average distance from the wall
// curDist=thisIsATest;
// if(i%100==0){
// printf("Difference: %f\n",avgDist-curDist);
// }
// if (i>=5&&avgDist-curDist<=36.0f) {
// float xPower, yPower;
// xPower=1;
// yPower=(avgDist-curDist)/36.0f;
// if(yPower>1){
// yPower=1;
// }
// driveRobot(yPower, xPower);
// setMotorValue(6, 1, 1);
// } else if (i>1400&&i<1600&&125>=potToDegrees(armPot.GetAverageVoltage())) {
// driveRobot(0, 0);
// shootRobot(1, false);
// setMotorValue(6, 1, 1);
// } else if (i>1500&&i<1700) {
// shootRobot(.1,false);
// //driveRobot(-1,0);
// } else {
// /*
// driveRobot(0, 0);
// shootRobot(0, true);
// setMotorValue(6, 1, 0);
// */
// }
// updateDashboard();
// 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",2);
// }
// Wait(0.005f);
// i++;
// cur++;
//}
} }
void OperatorControl() { void OperatorControl() {
myRobot.SetSafetyEnabled(false); myRobot.SetSafetyEnabled(false);
@ -390,7 +448,11 @@ public:
swap=!swap; swap=!swap;
} }
throttle = (-Lstick.GetRawAxis(4)+1)/2; throttle = (-Lstick.GetRawAxis(4)+1)/2;
driveRobot(Rstick.GetY(),Rstick.GetZ()+Rstick.GetX()); if(SmartDashboard::GetBoolean("Reverse Robot")){
driveRobot(-Rstick.GetY(),Rstick.GetZ()+Rstick.GetX());
}else{
driveRobot(Rstick.GetY(),Rstick.GetZ()+Rstick.GetX());
}
//Log things //Log things
if (i % 200 == 0) { if (i % 200 == 0) {
//logMsg(toString(compressor.GetPressureSwitchValue()),2); //logMsg(toString(compressor.GetPressureSwitchValue()),2);