From d702fe88f9984dd1a32aa3db808b4fdad5d6f745 Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Thu, 13 Mar 2014 16:59:56 -0700
Subject: [PATCH] Cleaned up code, removed logMsg

---
 MyRobot.cpp   | 37 +---------------------------
 SonarTest.cpp | 67 ---------------------------------------------------
 2 files changed, 1 insertion(+), 103 deletions(-)
 delete mode 100644 SonarTest.cpp

diff --git a/MyRobot.cpp b/MyRobot.cpp
index 848fd62..d43f33c 100644
--- a/MyRobot.cpp
+++ b/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);
     }
diff --git a/SonarTest.cpp b/SonarTest.cpp
deleted file mode 100644
index cf34c93..0000000
--- a/SonarTest.cpp
+++ /dev/null
@@ -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);