Default value changes, more working on auto

Commands/Autonomous/AutoDrive.cpp

@@ -4,13 +4,13 @@
 // Drive for a short while then stop. Just for testing
 AutoDrive::AutoDrive() : Command("AutoDrive"){
   Requires(DentRobot::drivetrain);
-  SetTimeout(1.0);
+  SetTimeout(0.5);
 }
 void AutoDrive::Initialize(){
 }
 void AutoDrive::Execute(){
   //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
-  DentRobot::drivetrain->DriveMecanum(0.5,0,0,0.9,0);
+  DentRobot::drivetrain->DriveMecanum(0.0,-.75,0.0,0.9,0.0);
 }
 bool AutoDrive::IsFinished(){
   return IsTimedOut();

Commands/Autonomous/Autonomous.cpp

@@ -1,9 +1,22 @@
 #include "Autonomous.h"
-#include "AutoDrive.h"
 #include "../../DentRobot.h"
 #include "../Elevator/Raise.h"
+#include "../Elevator/Lower.h"
+#include "AutoDrive.h"
+#include "Turn.h"
+#include "../Collector/CloseCollector.h"
+#include "../Collector/OpenCollector.h"
+#include "../Collector/CollectTote.h"
 Autonomous::Autonomous(){
-  AddSequential(new AutoDrive());
+  //AddSequential(new Raise());
+  //AddSequential(new Lower());
+  //AddSequential(new AutoDrive());
   AddSequential(new Raise());
+  AddSequential(new Lower());
+  AddSequential(new Turn());
+  AddParallel(new AutoDrive());
+  AddParallel(new CloseCollector());
+  AddParallel(new CollectTote());
+  AddSequential(new Turn());
 }
 // vim: ts=2:sw=2:et

Commands/Autonomous/Turn.cpp

@@ -0,0 +1,22 @@
+#include "Turn.h"
+#include "../../DentRobot.h"
+// Drive for a short while then stop. Just for testing
+Turn::Turn() : Command("Turn"){
+  Requires(DentRobot::drivetrain);
+  SetTimeout(0.25);
+}
+void Turn::Initialize(){
+}
+void Turn::Execute(){
+  //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
+  DentRobot::drivetrain->DriveMecanum(0.0,0.0,0.5,0.9,0.0);
+}
+bool Turn::IsFinished(){
+  return IsTimedOut();
+}
+void Turn::End(){
+}
+void Turn::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et

Commands/Autonomous/Turn.h

@@ -0,0 +1,19 @@
+#ifndef TURN_H
+#define TURN_H
+
+#include "Commands/Command.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class Turn: public Command{
+  public:
+    Turn();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+#endif
+// vim: ts=2:sw=2:et

Commands/Collector/CloseCollector.cpp

@@ -3,13 +3,21 @@ CloseCollector::CloseCollector() : Command("CloseCollector"){
   Requires(DentRobot::collector);
 }
 void CloseCollector::Initialize(){
-  SetTimeout(0.5);
+  printf("Initialized collector: 0.5\n");
+  SetTimeout(2.5);
 }
 void CloseCollector::Execute(){
-  DentRobot::collector->MoveArms(0.2f);
+  //printf("Closing collector: -0.5f\n");
+  DentRobot::collector->MoveArms(-0.5);
+  //DentRobot::collector->MoveArms(-(-DentRobot::oi->GetRightStick()->GetRawAxis(3)+1)/2*.3/.5);
 }
 bool CloseCollector::IsFinished(){
-  return DentRobot::collector->ArmSensor();
+  if(DentRobot::collector->ArmSensor()||IsTimedOut()){
+    printf("Stopped Closing: %d, %d\n",DentRobot::collector->ArmSensor(), IsTimedOut());
+    return true;
+  }else{
+    return false;
+  }
 }
 void CloseCollector::End(){
   DentRobot::collector->MoveArms(0.0f);

Commands/Collector/CollectTote.cpp

@@ -3,6 +3,7 @@ CollectTote::CollectTote() : Command("CollectTote"){
   Requires(DentRobot::collector);
 }
 void CollectTote::Initialize(){
+  printf("Initialized CollectTote\n");
   SetTimeout(2.0);
 }
 void CollectTote::Execute(){
@@ -10,7 +11,7 @@ void CollectTote::Execute(){
   DentRobot::collector->MoveRollers(-(-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2);
 }
 bool CollectTote::IsFinished(){
-  return DentRobot::collector->BoxCollected();
+  return DentRobot::collector->BoxCollected()||IsTimedOut();
 }
 void CollectTote::End(){
   DentRobot::collector->MoveRollers(0.0);

Commands/Collector/OpenCollector.cpp

@@ -6,11 +6,12 @@ void OpenCollector::Initialize(){
   SetTimeout(0.5);
 }
 void OpenCollector::Execute(){
-  //TODO check this value to move the motors in the right direction
-  DentRobot::collector->MoveArms(-0.2f);
+  DentRobot::collector->MoveArms(0.35);
+  //DentRobot::collector->MoveArms((-DentRobot::oi->GetRightStick()->GetRawAxis(3)+1)/2*.3/.5);
 }
 bool OpenCollector::IsFinished(){
-  return DentRobot::collector->ArmSensor();
+  //return DentRobot::collector->ArmSensor();
+  return IsTimedOut();
 }
 void OpenCollector::End(){
   DentRobot::collector->MoveArms(0.0f);

Commands/Drivetrain/Drive.cpp

@@ -12,12 +12,12 @@ void Drive::Execute(){
   x = DentRobot::oi->GetLeftStick()->GetRawAxis(0);
   z = DentRobot::oi->GetLeftStick()->GetRawAxis(2);
   y = DentRobot::oi->GetLeftStick()->GetRawAxis(1);
-  if (DentRobot::oi->GetLeftStick()->GetRawButton(1)){
-    x=0;
-  }
-  if (DentRobot::oi->GetLeftStick()->GetRawButton(2)){
-    y=0;
-  }
+  //if (DentRobot::oi->GetLeftStick()->GetRawButton(1)){
+  //  x=0;
+  //}
+  //if (DentRobot::oi->GetLeftStick()->GetRawButton(2)){
+  //  y=0;
+  //}
   //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
   DentRobot::drivetrain->DriveMecanum(x,y,z,0.9,0);
 }

Commands/Elevator/Lower.cpp

@@ -4,7 +4,7 @@
 Lower::Lower() : Command("Lower"){
 }
 void Lower::Initialize(){
-  SetTimeout(2.0);
+  SetTimeout(2.5);
 }
 void Lower::Execute(){
   DentRobot::elevator->Run(-1.0);

Commands/Elevator/Raise.cpp

@@ -4,7 +4,7 @@
 Raise::Raise() : Command("Raise"){
 }
 void Raise::Initialize(){
-  SetTimeout(2.5);
+  SetTimeout(3.0);
 }
 void Raise::Execute(){
   DentRobot::elevator->Run(1.0);

OI.cpp

@@ -13,12 +13,12 @@ OI::OI() {
   // Collector
   JoystickButton *right1=new JoystickButton(rightStick, 1);
   JoystickButton *right2=new JoystickButton(rightStick, 2);
-  JoystickButton *left3=new JoystickButton(leftStick, 3);
-  JoystickButton *left4=new JoystickButton(leftStick, 4);
-  right1->WhileHeld(new OpenCollector());
-  right2->WhileHeld(new CloseCollector());
-  left3->WhileHeld(new CollectTote());
-  left4->WhileHeld(new ReleaseTote());
+  JoystickButton *left1=new JoystickButton(leftStick, 1);
+  JoystickButton *left2=new JoystickButton(leftStick, 2);
+  right1->WhileHeld(new CloseCollector());
+  right2->WhileHeld(new OpenCollector());
+  left1->WhileHeld(new CollectTote());
+  left2->WhileHeld(new ReleaseTote());
   // Elevator
   Raise* raise=new Raise();
   Lower* lower=new Lower();