Added basic driving code (fing code (the stuff from the previous code). Doesn't prevent instant motor changes (ie: full forward to full reverse)

Definitions.h

@@ -66,4 +66,9 @@
 #define COLLECTOR_EXTEND        9
 #define COLLECTOR_RETRACT       10
 
+//Drive threshold definitions
+
+#define DRIVE_MAX_VALUE_CHANGE  .05
+#define DRIVE_SPEED_FALLBACK    2
+
 #endif

ZaphodBase.cpp

@@ -7,6 +7,12 @@ ZaphodBase::ZaphodBase():
 }
 void ZaphodBase::RobotInit()
 {
+  //Checks the state of the drive joystick to make sure it was not moved
+  //while plugged in, giving inaccurate readings
+  if(!zBot->checkJoystickValues())
+  {
+    printf("***UNPLUG AND REPLUG THE JOYSTICKS***\n"); 
+  }
 }
 void ZaphodBase::DisabledInit()
 {

ZaphodRobot.cpp

@@ -57,8 +57,35 @@ float ZaphodRobot::getRearSonar()
   return (rearSonarRightV+rearSonarLeftV)/2;
 }
 
-void driveRobot(float x, float y)
+bool ZaphodRobot::checkJoystickValues()
 {
+  float x = ControlSystem->rightJoystickAxisValues[1];
+  float y = ControlSystem->rightJoystickAxisValues[2];
+  if((-.1 < x && x < .1) && (-.1 < y && y < .1))
+  {
+    return true;
+  }
+  else
+  {
+    return false;
+  }
+}
+
+void ZaphodRobot::driveRobot(float x, float y)
+{ 
+    if(y>1.0f){
+    y=1.0f;
+    }else if(y!=0.0f&&y<-1.0f){
+    y=-1.0f;
+    }
+    float leftPower=((y+x)/2+1)*127+1;
+    float rightPower=((y-x)/2+1)*127+1;
+    right1->SetRaw(int(rightPower));
+    right2->SetRaw(int(rightPower));
+    right3->SetRaw(int(rightPower));
+    left1->SetRaw(int(leftPower));
+    left2->SetRaw(int(leftPower));
+    left3->SetRaw(int(leftPower));
 }
 
 //Main function used to handle periodic tasks on the robot
@@ -71,11 +98,11 @@ void ZaphodRobot::handler()
   //TODO Need to implement a timing system to not break the spike (this function doesn't run the compressor at the moment)
   compressorSystem->compressorSystemPeriodic();
   collector->updateCollector(shooter->isShooting, shooter->getAngle());
+  driveRobot(ControlSystem->rightJoystickAxisValues[3]+ControlSystem->rightJoystickAxisValues[1], -ControlSystem->rightJoystickAxisValues[2]);
   
   //Button assignments to actions
   if(ControlSystem->leftJoystickValues[SHOOTER_FIRE])
   {
-    //TODO Needs a power input
     shooter->startShootingSequence(ControlSystem->throttle);
   }
   if(ControlSystem->rightJoystickValues[COLLECTOR_INTAKE])

ZaphodRobot.h

@@ -22,10 +22,10 @@ class ZaphodRobot
     ZaphodCompressor *compressorSystem;
   public:
     ZaphodRobot();
-
     float frontSonarLeftV, frontSonarRightV, rearSonarLeftV, rearSonarRightV;
     float getFrontSonar();
     float getRearSonar();
+    bool checkJoystickValues();
     void driveRobot(float,float);
     void handler();
 };