From f6b6efacc5c2f7e1263adeb6a32aac36b2f107c4 Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Sun, 15 Feb 2015 17:54:59 -0500
Subject: [PATCH] Minor drive/auto improvements

---
 Commands/Autonomous/Autonomous.cpp | 1 +
 Commands/Autonomous/Turn.cpp       | 4 ++--
 Commands/Collector/CollectTote.cpp | 6 +-----
 Commands/Collector/RollIn.cpp      | 2 +-
 DentRobot.cpp                      | 2 +-
 Subsystems/Collector.cpp           | 4 ++--
 6 files changed, 8 insertions(+), 11 deletions(-)

diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp
index e13fac2..236a6b8 100644
--- a/Commands/Autonomous/Autonomous.cpp
+++ b/Commands/Autonomous/Autonomous.cpp
@@ -12,6 +12,7 @@ Autonomous::Autonomous(int seq){
     case 0:
       // Just for testing
       AddSequential(new CollectTote());
+      AddSequential(new Turn());
       //AddSequential(new Raise());
       //AddSequential(new Lower());
       //AddSequential(new Turn());
diff --git a/Commands/Autonomous/Turn.cpp b/Commands/Autonomous/Turn.cpp
index b42bd40..eea1450 100644
--- a/Commands/Autonomous/Turn.cpp
+++ b/Commands/Autonomous/Turn.cpp
@@ -3,13 +3,13 @@
 // Drive for a short while then stop. Just for testing
 Turn::Turn() : Command("Turn"){
   Requires(DentRobot::drivetrain);
-  SetTimeout(0.25);
+  SetTimeout(0.85);
 }
 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);
+  DentRobot::drivetrain->DriveMecanum(0.0,0.0,1.0,0.9,0.0);
 }
 bool Turn::IsFinished(){
   return IsTimedOut();
diff --git a/Commands/Collector/CollectTote.cpp b/Commands/Collector/CollectTote.cpp
index 39e134a..6b728f3 100644
--- a/Commands/Collector/CollectTote.cpp
+++ b/Commands/Collector/CollectTote.cpp
@@ -3,11 +3,7 @@
 #include "../Autonomous/AutoDrive.h"
 #include "RollIn.h"
 CollectTote::CollectTote(){
-  AddParallel(new AutoDrive(0.5, -1.0));
-  AddSequential(new RollIn());
-  AddParallel(new AutoDrive(0.5, 1.0));
-  AddSequential(new RollIn());
-  AddParallel(new AutoDrive(0.5, -1.0));
+  AddParallel(new AutoDrive(1.0, -0.75));
   AddSequential(new RollIn());
 }
 // vim: ts=2:sw=2:et
diff --git a/Commands/Collector/RollIn.cpp b/Commands/Collector/RollIn.cpp
index d7addc7..e4384f2 100644
--- a/Commands/Collector/RollIn.cpp
+++ b/Commands/Collector/RollIn.cpp
@@ -7,7 +7,7 @@ void RollIn::Initialize(){
 }
 void RollIn::Execute(){
   double throttle=DentRobot::oi->GetLeftThrottle();
-  double cvt=throttle*DentRobot::collector->GetSonarDistance()/0.4/5;
+  double cvt=throttle*DentRobot::collector->GetSonarDistance()/0.4;
   if(cvt>=1.0){
     DentRobot::collector->MoveRollers(1.0);
   }else{
diff --git a/DentRobot.cpp b/DentRobot.cpp
index 875799e..42a8201 100644
--- a/DentRobot.cpp
+++ b/DentRobot.cpp
@@ -18,7 +18,7 @@ DentRobot::DentRobot(){
   CameraServer::GetInstance()->StartAutomaticCapture("cam0");
   //SmartDashboard::PutNumber("Auto Wait Time", 1.0);
   //SmartDashboard::PutNumber("Auto Sequence", 0);
-  printf("Initialized");
+  printf("Initialized\n");
 }
 void DentRobot::RobotInit(){
   //SmartDashboard::PutNumber("CodeVersion",0.001);
diff --git a/Subsystems/Collector.cpp b/Subsystems/Collector.cpp
index a2322d4..f9b1f17 100644
--- a/Subsystems/Collector.cpp
+++ b/Subsystems/Collector.cpp
@@ -12,10 +12,10 @@ void Collector::InitDefaultCommand(){
 }
 void Collector::MoveRollers(double a){
   collectorMotorLeft->Set(a);
-  collectorMotorBottom->Set(a);
+  collectorMotorBottom->Set(-a);
   collectorMotorRamp->Set(a);
   collectorMotorRight->Set(-a);
-  printf("%f\n",GetSonarDistance());
+  printf("Roller power: %f\n",a);
 }
 double Collector::GetSonarDistance(){
   return sonarAnalog->GetAverageVoltage();