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

Added sonar test code, it works m8

Browse Source
This commit is contained in:
Austen Adler 2014-02-22 02:25:07 +00:00
parent ea82a9d89d
commit d9de47f247
1 changed files with 67 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
SonarTest.cpp Normal file
View File

@ -0,0 +1,67 @@
#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);