team2059/dent
2
0
Fork 0
mirror of https://github.com/team2059/Dent synced 2024-12-18 20:52:29 -05:00
Code Issues Releases Wiki Activity

Merge branch clenaup

Browse Source
This commit is contained in:
Adam Long 2015-02-28 08:28:39 +00:00
commit 60963fe2a6
15 changed files with 46 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Commands/Autonomous/AutoDrive.cpp
View File

@ -1,23 +1,23 @@
#include "AutoDrive.h"
#include "../../DentRobot.h"
// Drive for a short while then stop. Just for testing
AutoDrive::AutoDrive(double duration, double xtmp=-0.75, double ytmp=0) : Command("AutoDrive"){
AutoDrive::AutoDrive(double duration, double xtmp=0, double ytmp=-0.75) : Command("AutoDrive"){
Requires(DentRobot::drivetrain);
SetTimeout(duration);
speed=xtmp;
strafe=ytmp;
x=xtmp;
y=ytmp;
}
void AutoDrive::Initialize(){
}
void AutoDrive::Execute(){
//X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
DentRobot::drivetrain->DriveMecanum(strafe,speed,0.0,0.9,0.0);
DentRobot::drivetrain->DriveMecanum(x, y, 0.0, 0.9, 0.0);
}
bool AutoDrive::IsFinished(){
return IsTimedOut();
}
void AutoDrive::End(){
DentRobot::drivetrain->DriveMecanum(0.0,0.0,0.0,0.9,0.0);
DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.0, 0.9, 0.0);
}
void AutoDrive::Interrupted(){
End();

4
Commands/Autonomous/AutoDrive.h
View File

@ -8,10 +8,10 @@
class AutoDrive: public Command{
private:
double speed, strafe;
double x, y;
public:
AutoDrive(double);
AutoDrive(double, double,double);
AutoDrive(double, double, double);
void Initialize();
void Execute();
bool IsFinished();

26
Commands/Autonomous/Autonomous.cpp
View File

@ -15,44 +15,44 @@ Autonomous::Autonomous(int seq){
switch(seq){
case 0:
// Just for testing
AddSequential(new AutoDrive(.5,0,.25));
// Strafe at .25 power
AddSequential(new AutoDrive(0.5, 0.25, 0.0));
break;
case 1:
// Wait a desigated value, drive to Auto Zone (TM)
// Drive to Auto Zone (TM)
Wait(SmartDashboard::GetNumber("Auto Wait Time"));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.8,0.0));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.8));
break;
case 2:
// Wait a desigated value, drive to Auto Zone (TM)
// Collect a tote, turn, drive to Auto Zone (TM)
Wait(SmartDashboard::GetNumber("Auto Wait Time"));
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
AddSequential(new BinRaise(1.2));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"),0.75,0.0));
//AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0,-1.0));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.75));
AddSequential(new BinLower(1.0));
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
break;
case 3:
// Collect three totes, drive to Auto Zone (TM)
printf("Waiting: %f\n",SmartDashboard::GetNumber("Auto Wait Time"));
printf("Waiting: %f\n", SmartDashboard::GetNumber("Auto Wait Time"));
Wait(SmartDashboard::GetNumber("Auto Wait Time"));
printf("Done");
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), -0.75,0));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75));
AddSequential(new CollectTote());
AddSequential(new Raise());
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), -0.75,0));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75));
AddSequential(new CollectTote());
AddSequential(new Lower());
AddSequential(new Raise());
printf("Three totes?: %d\n",SmartDashboard::GetBoolean("Three totes"));
printf("Three totes?: %d\n", SmartDashboard::GetBoolean("Three totes"));
if(SmartDashboard::GetBoolean("Three totes")){
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), -0.75,0));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75));
AddSequential(new CollectTote());
AddSequential(new Lower());
AddSequential(new Raise());
}
AddSequential(new Turn(90));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), -0.75,0));
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0, 0.75));
AddSequential(new ReleaseTote());
break;
default:

2
Commands/Autonomous/CollectTote.cpp
View File

@ -3,7 +3,7 @@
#include "AutoDrive.h"
#include "../Collector/RollIn.h"
CollectTote::CollectTote(){
AddParallel(new AutoDrive(1.0, -0.75,0));
AddParallel(new AutoDrive(1.0, -0.75, 0.0));
AddSequential(new RollIn(1.0));
}
// vim: ts=2:sw=2:et

2
Commands/Autonomous/ReleaseTote.cpp
View File

@ -4,6 +4,6 @@
#include "../Collector/RollOut.h"
ReleaseTote::ReleaseTote(){
AddParallel(new RollOut());
AddParallel(new AutoDrive(0.5, 0.75,0));
AddParallel(new AutoDrive(0.5, 0, 0.75));
}
// vim: ts=2:sw=2:et

4
Commands/Autonomous/Turn.cpp
View File

@ -9,14 +9,14 @@ 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.6,0.9,0.0);
DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.6, 0.9, 0.0);
}
bool Turn::IsFinished(){
return IsTimedOut();
}
void Turn::End(){
// Stop driving
DentRobot::drivetrain->DriveMecanum(0.0,0.0,0.0,0.9,0.0);
DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.0, 0.9, 0.0);
}
void Turn::Interrupted(){
End();

2
Commands/Collector/RollOut.cpp
View File

@ -8,7 +8,7 @@ void RollOut::Initialize(){
void RollOut::Execute(){
//TODO check this value to move the motors in the right direction
// Devide by 2 twice because this speed should be half the collector speed
DentRobot::collector->MoveRollers(-DentRobot::oi->GetLeftThrottle()*.8);
DentRobot::collector->MoveRollers(-DentRobot::oi->GetLeftThrottle() * 0.8);
}
bool RollOut::IsFinished(){
return IsTimedOut();

8
Commands/Drivetrain/Drive.cpp
View File

@ -6,11 +6,11 @@ Drive::Drive() : Command("Drive"){
void Drive::Initialize(){
}
void Drive::Execute(){
double x,y,z;
//Code to lock the x axis when not holding button 1
double x, y, z;
x = DentRobot::oi->GetLeftStick()->GetRawAxis(0);
y = -DentRobot::oi->GetLeftStick()->GetRawAxis(1);
z = DentRobot::oi->GetLeftStick()->GetRawAxis(2);
y = DentRobot::oi->GetLeftStick()->GetRawAxis(1);
//Code to lock the x axis when not holding button 1
//if (DentRobot::oi->GetLeftStick()->GetRawButton(1)){
// x=0;
//}
@ -18,7 +18,7 @@ void Drive::Execute(){
// y=0;
//}
//X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
DentRobot::drivetrain->DriveMecanum(x,y,z,0.9,0);
DentRobot::drivetrain->DriveMecanum(x, y, z, 0.9, 0.0);
}
bool Drive::IsFinished(){
return IsTimedOut();

8
Commands/Test/CheckDrive.cpp
View File

@ -13,16 +13,16 @@ void CheckDrive::Initialize(){
void CheckDrive::Execute(){
switch(motor){
case DRIVE_FRONT_LEFT_CAN:
DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->FRONTLEFT,1);
DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->FRONTLEFT, 1);
break;
case DRIVE_FRONT_RIGHT_CAN:
DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->FRONTRIGHT,1);
DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->FRONTRIGHT, 1);
break;
case DRIVE_BACK_LEFT_CAN:
DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->BACKLEFT,1);
DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->BACKLEFT, 1);
break;
case DRIVE_BACK_RIGHT_CAN:
DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->BACKRIGHT,1);
DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->BACKRIGHT, 1);
break;
default:
break;

12
DentRobot.cpp
View File

@ -18,16 +18,14 @@ DentRobot::DentRobot(){
pneumatics=new Pneumatics();
//CameraServer::GetInstance()->SetQuality(25);
//CameraServer::GetInstance()->StartAutomaticCapture("cam0");
//SmartDashboard::PutNumber("Auto Wait Time", 1.0);
//SmartDashboard::PutNumber("Auto Sequence", 0);
printf("The robot is on\n");
}
void DentRobot::RobotInit(){
SmartDashboard::PutNumber("CodeVersion",CODE_VERSION);
SmartDashboard::PutNumber("CodeVersion", CODE_VERSION);
// Autonomous
// Sequence of autonomous command
SmartDashboard::PutNumber("Auto Sequence",2.0);
SmartDashboard::PutNumber("Auto Wait Time",3.0);
SmartDashboard::PutNumber("Auto Sequence", 2.0);
SmartDashboard::PutNumber("Auto Wait Time", 3.0);
// If the robot will be picking up three totes in sequence 3
SmartDashboard::PutBoolean("Three totes", true);
// TODO: Calibrate the following two values
@ -35,7 +33,7 @@ void DentRobot::RobotInit(){
SmartDashboard::PutNumber("Auto Zone Distance", 2.8);
// Distance (in time) to auto tote (used in sequence 3)
SmartDashboard::PutNumber("Auto Tote Distance", 0.5);
SmartDashboard::PutNumber("TurnAmount",2);
SmartDashboard::PutNumber("TurnAmount", 2.0);
// Elevators
SmartDashboard::PutBoolean("Bin Elevator Bottom", false);
@ -50,7 +48,7 @@ void DentRobot::DisabledPeriodic(){
}
void DentRobot::AutonomousInit(){
aut=new Autonomous(SmartDashboard::GetNumber("Auto Sequence"));
printf("Enabling Auto Sequence %f\n",SmartDashboard::GetNumber("Auto Sequence"));
printf("Enabling Auto Sequence %f\n", SmartDashboard::GetNumber("Auto Sequence"));
if(aut != NULL){
aut->Start();
}

2
Subsystems/BinElevator.cpp
View File

@ -2,7 +2,7 @@
#include "../RobotMap.h"
BinElevator::BinElevator(){
motor=new CANTalon(BINELEVATOR_CAN);
elevatorEncoder=new Encoder(BINELEVATOR_ENCODERA,BINELEVATOR_ENCODERB,false);
elevatorEncoder=new Encoder(BINELEVATOR_ENCODERA, BINELEVATOR_ENCODERB, false);
elevatorBottom=new DigitalInput(BINELEVATOR_BOTTOM_DIO);
elevatorTop=new DigitalInput(BINELEVATOR_TOP_DIO);
}

2
Subsystems/Collector.cpp
View File

@ -15,7 +15,7 @@ void Collector::MoveRollers(double a){
collectorMotorBottom->Set(-a);
collectorMotorRamp->Set(a);
collectorMotorRight->Set(-a);
printf("Roller power: %f\n",a);
printf("Roller power: %f\n", a);
}
double Collector::GetSonarDistance(){
return sonarAnalog->GetAverageVoltage();

6
Subsystems/Drivetrain.cpp
View File

@ -12,9 +12,9 @@ void Drivetrain::InitDefaultCommand(){
SetDefaultCommand(new Drive());
}
void Drivetrain::DriveMecanum(float x, float y, float z, float sensitivity, float gyro){
double correctY = (sensitivity*(pow(y,3))+(1-sensitivity)*y);
double correctX = -(sensitivity*(pow(x,3))+(1-sensitivity)*x);
double correctZ = -z *.5;
double correctX = -(sensitivity*(pow(x, 3))+(1-sensitivity)*x);
double correctY = -(sensitivity*(pow(y, 3))+(1-sensitivity)*y);
double correctZ = -z * 0.5;
if (DentRobot::oi->GetLeftStick()->GetRawButton(9)){
correctY /= SmartDashboard::GetNumber("DriveSpeedReductionThresh");
}

4
Subsystems/Drivetrain.h
View File

@ -15,9 +15,9 @@ class Drivetrain: public Subsystem{
BACKLEFT
};
void InitDefaultCommand();
void DriveMecanum(float,float,float,float,float);
void DriveMecanum(float, float, float, float, float);
void DriveArcade(float, float);
void TestMotor(e_motors,float);
void TestMotor(e_motors, float);
};
#endif
// vim: ts=2:sw=2:et

2
Subsystems/Elevator.cpp
View File

@ -2,7 +2,7 @@
#include "../RobotMap.h"
Elevator::Elevator(){
motor=new CANTalon(ELEVATOR_CAN);
elevatorEncoder=new Encoder(ELEVATOR_ENCODERA,ELEVATOR_ENCODERB,false);
elevatorEncoder=new Encoder(ELEVATOR_ENCODERA, ELEVATOR_ENCODERB, false);
elevatorBottom=new DigitalInput(ELEVATOR_BOTTOM_DIO);
elevatorMiddle=new DigitalInput(ELEVATOR_MIDDLE_DIO);
elevatorTop=new DigitalInput(ELEVATOR_TOP_DIO);