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

Cleaned up code, removed logMsg

Browse Source
This commit is contained in:
Austen Adler 2014-03-13 16:59:56 -07:00
parent fb883fa704
commit d702fe88f9
2 changed files with 1 additions and 103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
MyRobot.cpp
View File

@ -1,3 +1,4 @@
//Test comment!!
//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{{{
@ -142,21 +143,6 @@ public:
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());
}
}
//}}}
//toString{{{
template<typename numbertype> string toString(numbertype a) {
stringstream ss;
ss<<a;
string s = ss.str();
return s;
}
//}}}
//driveRobot{{{
void driveRobot(float x, float y) {
@ -167,10 +153,6 @@ public:
}
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);
@ -510,12 +492,10 @@ public:
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);
}
//}}}
i++;
@ -531,12 +511,9 @@ public:
//Initializations{{{
myRobot.SetSafetyEnabled(false);
int i = 0;
int cur=0;
bool swap=false;
collectorSole1.Set(true);
collectorSole2.Set(false);
compressing = false;
logMsg("Starting Teleop",1);
SmartDashboard::PutBoolean("CollectorState",false);
//}}}
while(IsEnabled() && IsOperatorControl()) {
@ -565,18 +542,13 @@ public:
if(Lstick.GetRawButton(1)==1) {
//Shoot{{{
shooting = true;
logMsg("Firing",13);
logMsg("Collector is extended, going to fire",17);
shootRobot(throttle);
setMotorValue(6, 1, 1);
if(collectorExtended == false) {
shooting = false;
logMsg("Collector is NOT extended, not going to fire",17);
}
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);
}
@ -585,12 +557,8 @@ public:
//Lower Shooter{{{
shooting = false;
shootRobot(-0.1f);
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 {
@ -644,17 +612,14 @@ public:
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);
}

67
SonarTest.cpp
View File

@ -1,67 +0,0 @@
#include "WPILib.h"
#include "SmartDashboard/SmartDashboard.h"
#include "Command.h"
#include <iostream>
#include <math.h>
#include <vector>
#include <sstream>
class RobotDemo : public SimpleRobot
{
RobotDrive myRobot;
DigitalOutput out,out2;
Jaguar Left1,Left2,Right1,Right2;
AnalogChannel BallSonicLeft,BallSonicRight;
public:
RobotDemo():
out(1),
out2(2),
Left1(1),
Left2(2),
Right1(3),
Right2(4),
BallSonicLeft(1),
BallSonicRight(2),
myRobot(Left1,Left2,Right1,Right2){
GetWatchdog().SetEnabled(false);
}
void RobotInit(){
SmartDashboard::PutNumber("Ball Left", voltToDistance(BallSonicLeft.GetAverageVoltage()));
SmartDashboard::PutNumber("AtanValue", atan((voltToDistance(BallSonicLeft.GetAverageVoltage()) - voltToDistance(BallSonicRight.GetAverageVoltage()))/20.0f)/(2*3.14159265358979)*360);
}
void Test(){
while(IsEnabled()&&IsTest()){
}
}
void Autonomous(){
myRobot.SetSafetyEnabled(false);
bool swap=true;
int i=0;
int cur=0;
while(IsEnabled()&&IsAutonomous()){
if(cur==50){
cur=0;
if(swap){
out.Set(1);
out2.Set(0);
}else{
out.Set(0);
out2.Set(1);
}
printf("Swap: %d\n",swap);
swap=!swap;
}
SmartDashboard::PutNumber("Ball Left", voltToDistance(BallSonicLeft.GetAverageVoltage()));
SmartDashboard::PutNumber("Ball Right", voltToDistance(BallSonicRight.GetAverageVoltage()));
SmartDashboard::PutNumber("AtanValue", atan((abs(voltToDistance(BallSonicLeft.GetAverageVoltage()) - voltToDistance(BallSonicRight.GetAverageVoltage())))/20.0f)*360.0f/(2.0f*3.141592653589793f));
i++;
cur++;
Wait(0.005f);
}
}
void OperatorControl(){
}
float voltToDistance(float a) {
return (a / 0.000976562f) / 25.4f;
}
};
START_ROBOT_CLASS(RobotDemo);