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

Fixed up code after car voyage... Preparing to add ultrasonic code

Browse Source
This commit is contained in:
Austen Adler 2014-02-27 04:02:49 +00:00
parent 938a8b74f8
commit 0b57a289ce
1 changed files with 125 additions and 193 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

118
MyRobot.cpp
View File

@ -1,4 +1,3 @@
//TODO this
//Sonar code
#include "WPILib.h"
#include "SmartDashboard/SmartDashboard.h"
@ -27,8 +26,12 @@ class RobotDemo : public SimpleRobot
//Ultrasonic
AnalogChannel BallSonicLeft, BallSonicRight, WallSonicLeft, WallSonicRight;
DigitalOutput BallLeft, BallRight, WallLeft, WallRight;
//DigitalInput ballLimit;
public:
RobotDemo():
//Limit Switches
//TODO
//ballLimit(),
//Joysticks
Rstick(1),
Lstick(2),
@ -44,8 +47,8 @@ public:
BallRight(2,4),
WallRight(2,5),
//Compressor
//compressor(2,3,1,1),
compressor(2, 5, 1, 1), //Solenoids
compressor(2, 5, 1, 1),
//Solenoids
collectorSole1(1),
collectorSole2(2),
//Limit switches
@ -105,7 +108,7 @@ public:
SmartDashboard::PutNumber("Ball Left", voltToDistance(BallSonicLeft.GetAverageVoltage()));
SmartDashboard::PutNumber("Ball Right", voltToDistance(BallSonicRight.GetAverageVoltage()));
SmartDashboard::PutBoolean("Use Ultrasonic",false);
SmartDashboard::PutBoolean("Reverse Robot",false);
SmartDashboard::PutBoolean("Daniel Mode",false);
}
void updateDashboard() {
SmartDashboard::PutNumber("Throttle", throttle);
@ -269,8 +272,6 @@ public:
float yoyo = atan((abs(voltToDistance(BallSonicLeft.GetAverageVoltage()) - voltToDistance(BallSonicRight.GetAverageVoltage())))/20.0f)*360.0f/(2.0f*3.141592653589793f);
SmartDashboard::PutNumber("Distance", yoyo);
}
// float getSonar(int sonar){
// switch(sonar){
void Test() {
}
void Autonomous() {
@ -321,20 +322,25 @@ public:
}
} else {
if(i<=initalDriveTime*200) {
setMotorValue(6, 1, 1);
driveRobot(1.0f,0.0f);
//setMotorValue(6, 1, 1);
} else if(i>shooterDelay&&i<shooterDelay+(shooterDuration*200)&&shooterMaxAngle>=potToDegrees(armPot.GetAverageVoltage())) {
driveRobot(0, 0);
shootRobot(1, true);
setMotorValue(6, 1, 1);
} else if (i>1500&&i<1700) {
shootRobot(.1,true);
//driveRobot(-1,0);
} else {
/*
driveRobot(0, 0);
shootRobot(0, true);
setMotorValue(6, 1, 0);
*/
}
if(i>480&&i<430+initalDriveTime){
driveRobot(-1,0);
//TODO
} else if(i>430+initalDriveTime/*&&ballLimit.Get()==1*/) {
driveRobot(-0.6f,0.0f);
setMotorValue(6, 1, 1);
//TODO
} else if(/*ballLimit.Get()==1&&*/i>430+initalDriveTime) {
driveRobot(1.0f,0.0f);
}
if(i>480&&i<480){
shootRobot(0.25f,true);
}
}
updateDashboard();
@ -354,80 +360,6 @@ public:
}
compressing = false;
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() {
myRobot.SetSafetyEnabled(false);
@ -448,7 +380,7 @@ public:
swap=!swap;
}
throttle = (-Lstick.GetRawAxis(4)+1)/2;
if(SmartDashboard::GetBoolean("Reverse Robot")){
if(SmartDashboard::GetBoolean("Daniel Mode")) {
driveRobot(-Rstick.GetY(),Rstick.GetZ()+Rstick.GetX());
} else {
driveRobot(Rstick.GetY(),Rstick.GetZ()+Rstick.GetX());
@ -497,7 +429,8 @@ public:
if(collectorExtended == true) {
shootRobot(-DownSpeed, false);
logMsg("Collector is extended, going to fire",17);
} } else {
}
} else {
shooting = false;
//Stop all motors
shootRobot(0, true);
@ -525,4 +458,3 @@ public:
}
};
START_ROBOT_CLASS(RobotDemo);