Cleaned up code, removed logMsg

MyRobot.cpp

@@ -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);
     }

SonarTest.cpp

@@ -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);