Arrowhead
25-09-2013, 16:30
So lately our team has been having problems downloading code to our robot. Sometimes we will upload the code, but the robot code light will not light up. We eventually figured out that this was happening whenever we made any PID controllers in our code (used to control motor speed). Here is the robot code with PID controllers, which doesn't work.
/*
Minotaur FRC 1369
2013 Robot Code
Lead Cracker: Graham Peterson
Lead Lead: Daniel Brown
*Insert Important Stuff Here*
*/
//after season 2013
#include "WPILib.h"
#include <math.h>
float p=0;
float i=0;
float d=0;
float period=0;
class RobotDemo : public SimpleRobot
{
Victor fl; //front left motor:V1
Victor bl; //back left motor:V2
Victor fr; //front right motor:V3
Victor br; //back right motor:V4
RobotDrive rDrive; //declare robot drive
Victor shooterVictor; // front victor for shooter
Victor shooterVictor2; //back
Solenoid loaderArm; //declare servo for loader arm
Joystick driverStick; //declares all 3 joysticks
Joystick driverStick2;
Joystick shooterStick;
Solenoid SC1; //declares solenoids for climbing arm
Solenoid SC2;
Relay ts;
Encoder efl;
PIDController pfl;
Encoder efr;
PIDController pfr;
Encoder ebl;
PIDController pbl;
Encoder ebr;
PIDController pbr;
DriverStation* driverStation;
public:
RobotDemo(void):
fl(1), //assign drive motors PWM ports
bl(2),
fr(3),
br(4),
rDrive(fl, bl, fr, br), //assigns robot drive victors theiw PWM ports
shooterVictor(6), //assigns victor its PWM port
shooterVictor2(7),
loaderArm(3), //assign servo its PWM port
driverStick(1), //assign joysticks their USB ports
driverStick2(2),
shooterStick(3),
SC1(1), //assigns climbing pneumatics digital output ports
SC2(2),
ts(8,Relay::kForwardOnly),
efl(2,3,false,4),
pfl(p,i,d,&efl,&fl,period),
efr(4,5,false,4),
pfr(p,i,d,&efr,&fr,period),
ebl(6,7,false,4),
pbl(p,i,d,&ebl,&bl,period),
ebr(8,9,false,4),
pbr(p,i,d,&ebr,&br,period)
{
rDrive.SetInvertedMotor(RobotDrive::kRearRightMoto r, true);
rDrive.SetInvertedMotor(RobotDrive::kFrontRightMot or, true);
driverStation = DriverStation::GetInstance();
Compressor *c = new Compressor(1, 2);
c->Start();
Watchdog().SetEnabled(false);
rDrive.SetExpiration(0.1);
rDrive.SetSafetyEnabled(true); //sets motor saftey on for all motors
shooterVictor.SetExpiration(0.1);
shooterVictor.SetSafetyEnabled(true);
shooterVictor2.SetExpiration(0.1);
shooterVictor2.SetSafetyEnabled(true);
}
void Autonomous(void)
{
DriverStationLCD *ds = DriverStationLCD::GetInstance();
ds->Clear();
rDrive.SetSafetyEnabled(false); //turn of motor saftey timers during autonomous for convience
shooterVictor.SetSafetyEnabled(false);
shooterVictor2.SetSafetyEnabled(false);
ds->PrintfLine(DriverStationLCD::kUser_Line1, "Robot Is in Autonomous");
ds->UpdateLCD();
shooterVictor.Set(-1.0);
shooterVictor2.Set(-1.0);
Wait(5.0);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
shooterVictor.Set(0);
shooterVictor2.Set(0);
/*
shooterVictor.Set(-1.0);
shooterVictor2.Set(-1.0);
Wait(1.0);
rDrive.MecanumDrive_Cartesian(0.2, 0.0, 0.0);
Wait(0.5);
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
Wait(3.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
shooterVictor.Set(0);
shooterVictor2.Set(0);
*/
while(IsAutonomous()){
Wait(.005);
}
}
void OperatorControl(void){
rDrive.SetSafetyEnabled(true); //turn motor safety timers back on
shooterVictor.SetSafetyEnabled(true);
shooterVictor2.SetSafetyEnabled(true);
DriverStationLCD *ds = DriverStationLCD::GetInstance();
ds->UpdateLCD();
while (IsOperatorControl()){
ds->PrintfLine(DriverStationLCD::kUser_Line1, "Robot Is In TeleOp");
driving(); //runs drive control function
ShooterControl(); //runs shooter control function
LoaderControl(); //runs servo control function
Climbing(); //runs pryamid climbing function
t();
float dfl=efl.Get();
float dfr=efr.Get();
float dbl=ebl.Get();
float dbr=ebr.Get();
ds->PrintfLine(DriverStationLCD::kUser_Line2, "%f",dfl);
ds->PrintfLine(DriverStationLCD::kUser_Line3, "%f",dfr);
ds->PrintfLine(DriverStationLCD::kUser_Line4, "%f",dbl);
ds->PrintfLine(DriverStationLCD::kUser_Line5, "%f",dbr);
ds->UpdateLCD();
Wait(0.05); //waits 0.02 seconds
}
}
void t(){
if(shooterStick.GetRawButton(2)){
ts.Set(Relay::kForward);
}
else{
ts.Set(Relay::kOff);
}
}
void driving(){
double y = driverStick.GetY(); //variable for forward/backward movement
double x = driverStick.GetX(); //variable for side to side movement
double turn = driverStick2.GetX() ; //variable for turning movement
deadzone(y);
deadzone(x);
deadzone(turn);
mecanum(y, -x, turn);
}
double deadzone(double in){
double f_in=fabs(in);
if(in!=0){
double f_in=fabs(in);
f_in=1.25*f_in;
f_in=f_in-.25;
if(in<0){
f_in=f_in*-1;
}
}
return f_in;
}
void mecanum(float y,float x,float turn){
y=-y;
float mfl,mfr,mbl,mbr;
mfl=y+x+turn;
mfr=y-x-turn;
mbl=y-x+turn;
mbr=x+y-turn;
float max=fabs(mfl);
if(fabs(mfr)>max){
max=fabs(mfr);
}
if(fabs(mbl)>max){
max=fabs(mbl);
}
if(fabs(mbr)>max){
max=fabs(mbr);
}
mfl=mfl/max;
mfr=mfr/max;
mbl=mbl/max;
mbr=mbr/max;
pfl.SetSetpoint(mfl);
pfr.SetSetpoint(mfr);
pbl.SetSetpoint(mbl);
pbr.SetSetpoint(mbr);
}
void ShooterControl(){ //shooter control function
if(shooterStick.GetRawButton(1)){
shooterVictor.Set(-1.0); //if trigger is pressed, turn on shooter
shooterVictor2.Set(-1.0);
}
else{
shooterVictor.Set(0); //if trigger is pressed, turn on shooter
shooterVictor2.Set(0);
}
}
void LoaderControl(){ //servo control function
if(shooterStick.GetRawButton(3) && shooterStick.GetRawButton(1)){
loaderArm.Set(true); //if middle button is pressed, extend servo
}
else{
loaderArm.Set(false);
}
}
void Climbing(){ //pyramid climbing function, happens automatically
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if( shooterStick.GetRawButton(4)){ //if button 6 pressed on both sticks, starts climb
ds->PrintfLine(DriverStationLCD::kUser_Line6, "Arms Up");
SC1.Set(true);
SC2.Set(true);
}
if(shooterStick.GetRawButton(5)){
ds->PrintfLine(DriverStationLCD::kUser_Line6, "Arms Down");
SC1.Set(false);
SC2.Set(false);
}
}
void Test() {
shooterVictor.Set(0);
shooterVictor2.Set(0);
Compressor *c = new Compressor(3, 2);
c->Start();
Wait(0.05);
}
};
START_ROBOT_CLASS(RobotDemo); //starts the class running
and here is the code that does work, the only difference being that the PID controllers are taken out.
/*
Minotaur FRC 1369
2013 Robot Code
Lead Cracker: Graham Peterson
Lead Lead: Daniel Brown
*Insert Important Stuff Here*
*/
//after season 2013
#include "WPILib.h"
#include <math.h>
float p=0;
float i=0;
float d=0;
float period=0;
class RobotDemo : public SimpleRobot
{
Victor fl; //front left motor:V1
Victor bl; //back left motor:V2
Victor fr; //front right motor:V3
Victor br; //back right motor:V4
RobotDrive rDrive; //declare robot drive
Victor shooterVictor; // front victor for shooter
Victor shooterVictor2; //back
Solenoid loaderArm; //declare servo for loader arm
Joystick driverStick; //declares all 3 joysticks
Joystick driverStick2;
Joystick shooterStick;
Solenoid SC1; //declares solenoids for climbing arm
Solenoid SC2;
Relay ts;
Encoder efl;
DriverStation* driverStation;
public:
RobotDemo(void):
fl(1), //assign drive motors PWM ports
bl(2),
fr(3),
br(4),
rDrive(fl, bl, fr, br), //assigns robot drive victors theiw PWM ports
shooterVictor(6), //assigns victor its PWM port
shooterVictor2(7),
loaderArm(3), //assign servo its PWM port
driverStick(1), //assign joysticks their USB ports
driverStick2(2),
shooterStick(3),
SC1(1), //assigns climbing pneumatics digital output ports
SC2(2),
ts(8,Relay::kForwardOnly),
efl(2,3,false,4)
{
rDrive.SetInvertedMotor(RobotDrive::kRearRightMoto r, true);
rDrive.SetInvertedMotor(RobotDrive::kFrontRightMot or, true);
driverStation = DriverStation::GetInstance();
Compressor *c = new Compressor(1, 2);
c->Start();
Watchdog().SetEnabled(false);
rDrive.SetExpiration(0.1);
rDrive.SetSafetyEnabled(true); //sets motor saftey on for all motors
shooterVictor.SetExpiration(0.1);
shooterVictor.SetSafetyEnabled(true);
shooterVictor2.SetExpiration(0.1);
shooterVictor2.SetSafetyEnabled(true);
}
void Autonomous(void)
{
DriverStationLCD *ds = DriverStationLCD::GetInstance();
ds->Clear();
rDrive.SetSafetyEnabled(false); //turn of motor saftey timers during autonomous for convience
shooterVictor.SetSafetyEnabled(false);
shooterVictor2.SetSafetyEnabled(false);
ds->PrintfLine(DriverStationLCD::kUser_Line1, "Robot Is in Autonomous");
ds->UpdateLCD();
shooterVictor.Set(-1.0);
shooterVictor2.Set(-1.0);
Wait(5.0);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
shooterVictor.Set(0);
shooterVictor2.Set(0);
/*
shooterVictor.Set(-1.0);
shooterVictor2.Set(-1.0);
Wait(1.0);
rDrive.MecanumDrive_Cartesian(0.2, 0.0, 0.0);
Wait(0.5);
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
Wait(3.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
shooterVictor.Set(0);
shooterVictor2.Set(0);
*/
while(IsAutonomous()){
Wait(.005);
}
}
void OperatorControl(void){
rDrive.SetSafetyEnabled(true); //turn motor safety timers back on
shooterVictor.SetSafetyEnabled(true);
shooterVictor2.SetSafetyEnabled(true);
DriverStationLCD *ds = DriverStationLCD::GetInstance();
ds->UpdateLCD();
while (IsOperatorControl()){
ds->PrintfLine(DriverStationLCD::kUser_Line1, "Robot Is In TeleOp");
driving(); //runs drive control function
ShooterControl(); //runs shooter control function
LoaderControl(); //runs servo control function
Climbing(); //runs pryamid climbing function
t();
float dfl=efl.Get();
ds->PrintfLine(DriverStationLCD::kUser_Line2, "%f",dfl);
Wait(0.05); //waits 0.02 seconds
}
}
void t(){
if(shooterStick.GetRawButton(2)){
ts.Set(Relay::kForward);
}
else{
ts.Set(Relay::kOff);
}
}
void driving(){
double y = driverStick.GetY(); //variable for forward/backward movement
double x = driverStick.GetX(); //variable for side to side movement
double turn = driverStick2.GetX() ; //variable for turning movement
deadzone(y);
deadzone(x);
deadzone(turn);
mecanum(y, -x, turn);
}
double deadzone(double in){
double f_in=fabs(in);
if(in!=0){
double f_in=fabs(in);
f_in=1.25*f_in;
f_in=f_in-.25;
if(in<0){
f_in=f_in*-1;
}
}
return f_in;
}
void mecanum(float y,float x,float turn){
y=-y;
float mfl,mfr,mbl,mbr;
mfl=y+x+turn;
mfr=y-x-turn;
mbl=y-x+turn;
mbr=x+y-turn;
float max=fabs(mfl);
if(fabs(mfr)>max){
max=fabs(mfr);
}
if(fabs(mbl)>max){
max=fabs(mbl);
}
if(fabs(mbr)>max){
max=fabs(mbr);
}
mfl=mfl/max;
mfr=mfr/max;
mbl=mbl/max;
mbr=mbr/max;
}
void ShooterControl(){ //shooter control function
if(shooterStick.GetRawButton(1)){
shooterVictor.Set(-1.0); //if trigger is pressed, turn on shooter
shooterVictor2.Set(-1.0);
}
else{
shooterVictor.Set(0); //if trigger is pressed, turn on shooter
shooterVictor2.Set(0);
}
}
void LoaderControl(){ //servo control function
if(shooterStick.GetRawButton(3) && shooterStick.GetRawButton(1)){
loaderArm.Set(true); //if middle button is pressed, extend servo
}
else{
loaderArm.Set(false);
}
}
void Climbing(){ //pyramid climbing function, happens automatically
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if( shooterStick.GetRawButton(4)){ //if button 6 pressed on both sticks, starts climb
ds->PrintfLine(DriverStationLCD::kUser_Line6, "Arms Up");
SC1.Set(true);
SC2.Set(true);
}
if(shooterStick.GetRawButton(5)){
ds->PrintfLine(DriverStationLCD::kUser_Line6, "Arms Down");
SC1.Set(false);
SC2.Set(false);
}
}
void Test() {
shooterVictor.Set(0);
shooterVictor2.Set(0);
Compressor *c = new Compressor(3, 2);
c->Start();
Wait(0.05);
}
};
START_ROBOT_CLASS(RobotDemo); //starts the class running
/*
Minotaur FRC 1369
2013 Robot Code
Lead Cracker: Graham Peterson
Lead Lead: Daniel Brown
*Insert Important Stuff Here*
*/
//after season 2013
#include "WPILib.h"
#include <math.h>
float p=0;
float i=0;
float d=0;
float period=0;
class RobotDemo : public SimpleRobot
{
Victor fl; //front left motor:V1
Victor bl; //back left motor:V2
Victor fr; //front right motor:V3
Victor br; //back right motor:V4
RobotDrive rDrive; //declare robot drive
Victor shooterVictor; // front victor for shooter
Victor shooterVictor2; //back
Solenoid loaderArm; //declare servo for loader arm
Joystick driverStick; //declares all 3 joysticks
Joystick driverStick2;
Joystick shooterStick;
Solenoid SC1; //declares solenoids for climbing arm
Solenoid SC2;
Relay ts;
Encoder efl;
PIDController pfl;
Encoder efr;
PIDController pfr;
Encoder ebl;
PIDController pbl;
Encoder ebr;
PIDController pbr;
DriverStation* driverStation;
public:
RobotDemo(void):
fl(1), //assign drive motors PWM ports
bl(2),
fr(3),
br(4),
rDrive(fl, bl, fr, br), //assigns robot drive victors theiw PWM ports
shooterVictor(6), //assigns victor its PWM port
shooterVictor2(7),
loaderArm(3), //assign servo its PWM port
driverStick(1), //assign joysticks their USB ports
driverStick2(2),
shooterStick(3),
SC1(1), //assigns climbing pneumatics digital output ports
SC2(2),
ts(8,Relay::kForwardOnly),
efl(2,3,false,4),
pfl(p,i,d,&efl,&fl,period),
efr(4,5,false,4),
pfr(p,i,d,&efr,&fr,period),
ebl(6,7,false,4),
pbl(p,i,d,&ebl,&bl,period),
ebr(8,9,false,4),
pbr(p,i,d,&ebr,&br,period)
{
rDrive.SetInvertedMotor(RobotDrive::kRearRightMoto r, true);
rDrive.SetInvertedMotor(RobotDrive::kFrontRightMot or, true);
driverStation = DriverStation::GetInstance();
Compressor *c = new Compressor(1, 2);
c->Start();
Watchdog().SetEnabled(false);
rDrive.SetExpiration(0.1);
rDrive.SetSafetyEnabled(true); //sets motor saftey on for all motors
shooterVictor.SetExpiration(0.1);
shooterVictor.SetSafetyEnabled(true);
shooterVictor2.SetExpiration(0.1);
shooterVictor2.SetSafetyEnabled(true);
}
void Autonomous(void)
{
DriverStationLCD *ds = DriverStationLCD::GetInstance();
ds->Clear();
rDrive.SetSafetyEnabled(false); //turn of motor saftey timers during autonomous for convience
shooterVictor.SetSafetyEnabled(false);
shooterVictor2.SetSafetyEnabled(false);
ds->PrintfLine(DriverStationLCD::kUser_Line1, "Robot Is in Autonomous");
ds->UpdateLCD();
shooterVictor.Set(-1.0);
shooterVictor2.Set(-1.0);
Wait(5.0);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
shooterVictor.Set(0);
shooterVictor2.Set(0);
/*
shooterVictor.Set(-1.0);
shooterVictor2.Set(-1.0);
Wait(1.0);
rDrive.MecanumDrive_Cartesian(0.2, 0.0, 0.0);
Wait(0.5);
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
Wait(3.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
shooterVictor.Set(0);
shooterVictor2.Set(0);
*/
while(IsAutonomous()){
Wait(.005);
}
}
void OperatorControl(void){
rDrive.SetSafetyEnabled(true); //turn motor safety timers back on
shooterVictor.SetSafetyEnabled(true);
shooterVictor2.SetSafetyEnabled(true);
DriverStationLCD *ds = DriverStationLCD::GetInstance();
ds->UpdateLCD();
while (IsOperatorControl()){
ds->PrintfLine(DriverStationLCD::kUser_Line1, "Robot Is In TeleOp");
driving(); //runs drive control function
ShooterControl(); //runs shooter control function
LoaderControl(); //runs servo control function
Climbing(); //runs pryamid climbing function
t();
float dfl=efl.Get();
float dfr=efr.Get();
float dbl=ebl.Get();
float dbr=ebr.Get();
ds->PrintfLine(DriverStationLCD::kUser_Line2, "%f",dfl);
ds->PrintfLine(DriverStationLCD::kUser_Line3, "%f",dfr);
ds->PrintfLine(DriverStationLCD::kUser_Line4, "%f",dbl);
ds->PrintfLine(DriverStationLCD::kUser_Line5, "%f",dbr);
ds->UpdateLCD();
Wait(0.05); //waits 0.02 seconds
}
}
void t(){
if(shooterStick.GetRawButton(2)){
ts.Set(Relay::kForward);
}
else{
ts.Set(Relay::kOff);
}
}
void driving(){
double y = driverStick.GetY(); //variable for forward/backward movement
double x = driverStick.GetX(); //variable for side to side movement
double turn = driverStick2.GetX() ; //variable for turning movement
deadzone(y);
deadzone(x);
deadzone(turn);
mecanum(y, -x, turn);
}
double deadzone(double in){
double f_in=fabs(in);
if(in!=0){
double f_in=fabs(in);
f_in=1.25*f_in;
f_in=f_in-.25;
if(in<0){
f_in=f_in*-1;
}
}
return f_in;
}
void mecanum(float y,float x,float turn){
y=-y;
float mfl,mfr,mbl,mbr;
mfl=y+x+turn;
mfr=y-x-turn;
mbl=y-x+turn;
mbr=x+y-turn;
float max=fabs(mfl);
if(fabs(mfr)>max){
max=fabs(mfr);
}
if(fabs(mbl)>max){
max=fabs(mbl);
}
if(fabs(mbr)>max){
max=fabs(mbr);
}
mfl=mfl/max;
mfr=mfr/max;
mbl=mbl/max;
mbr=mbr/max;
pfl.SetSetpoint(mfl);
pfr.SetSetpoint(mfr);
pbl.SetSetpoint(mbl);
pbr.SetSetpoint(mbr);
}
void ShooterControl(){ //shooter control function
if(shooterStick.GetRawButton(1)){
shooterVictor.Set(-1.0); //if trigger is pressed, turn on shooter
shooterVictor2.Set(-1.0);
}
else{
shooterVictor.Set(0); //if trigger is pressed, turn on shooter
shooterVictor2.Set(0);
}
}
void LoaderControl(){ //servo control function
if(shooterStick.GetRawButton(3) && shooterStick.GetRawButton(1)){
loaderArm.Set(true); //if middle button is pressed, extend servo
}
else{
loaderArm.Set(false);
}
}
void Climbing(){ //pyramid climbing function, happens automatically
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if( shooterStick.GetRawButton(4)){ //if button 6 pressed on both sticks, starts climb
ds->PrintfLine(DriverStationLCD::kUser_Line6, "Arms Up");
SC1.Set(true);
SC2.Set(true);
}
if(shooterStick.GetRawButton(5)){
ds->PrintfLine(DriverStationLCD::kUser_Line6, "Arms Down");
SC1.Set(false);
SC2.Set(false);
}
}
void Test() {
shooterVictor.Set(0);
shooterVictor2.Set(0);
Compressor *c = new Compressor(3, 2);
c->Start();
Wait(0.05);
}
};
START_ROBOT_CLASS(RobotDemo); //starts the class running
and here is the code that does work, the only difference being that the PID controllers are taken out.
/*
Minotaur FRC 1369
2013 Robot Code
Lead Cracker: Graham Peterson
Lead Lead: Daniel Brown
*Insert Important Stuff Here*
*/
//after season 2013
#include "WPILib.h"
#include <math.h>
float p=0;
float i=0;
float d=0;
float period=0;
class RobotDemo : public SimpleRobot
{
Victor fl; //front left motor:V1
Victor bl; //back left motor:V2
Victor fr; //front right motor:V3
Victor br; //back right motor:V4
RobotDrive rDrive; //declare robot drive
Victor shooterVictor; // front victor for shooter
Victor shooterVictor2; //back
Solenoid loaderArm; //declare servo for loader arm
Joystick driverStick; //declares all 3 joysticks
Joystick driverStick2;
Joystick shooterStick;
Solenoid SC1; //declares solenoids for climbing arm
Solenoid SC2;
Relay ts;
Encoder efl;
DriverStation* driverStation;
public:
RobotDemo(void):
fl(1), //assign drive motors PWM ports
bl(2),
fr(3),
br(4),
rDrive(fl, bl, fr, br), //assigns robot drive victors theiw PWM ports
shooterVictor(6), //assigns victor its PWM port
shooterVictor2(7),
loaderArm(3), //assign servo its PWM port
driverStick(1), //assign joysticks their USB ports
driverStick2(2),
shooterStick(3),
SC1(1), //assigns climbing pneumatics digital output ports
SC2(2),
ts(8,Relay::kForwardOnly),
efl(2,3,false,4)
{
rDrive.SetInvertedMotor(RobotDrive::kRearRightMoto r, true);
rDrive.SetInvertedMotor(RobotDrive::kFrontRightMot or, true);
driverStation = DriverStation::GetInstance();
Compressor *c = new Compressor(1, 2);
c->Start();
Watchdog().SetEnabled(false);
rDrive.SetExpiration(0.1);
rDrive.SetSafetyEnabled(true); //sets motor saftey on for all motors
shooterVictor.SetExpiration(0.1);
shooterVictor.SetSafetyEnabled(true);
shooterVictor2.SetExpiration(0.1);
shooterVictor2.SetSafetyEnabled(true);
}
void Autonomous(void)
{
DriverStationLCD *ds = DriverStationLCD::GetInstance();
ds->Clear();
rDrive.SetSafetyEnabled(false); //turn of motor saftey timers during autonomous for convience
shooterVictor.SetSafetyEnabled(false);
shooterVictor2.SetSafetyEnabled(false);
ds->PrintfLine(DriverStationLCD::kUser_Line1, "Robot Is in Autonomous");
ds->UpdateLCD();
shooterVictor.Set(-1.0);
shooterVictor2.Set(-1.0);
Wait(5.0);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
shooterVictor.Set(0);
shooterVictor2.Set(0);
/*
shooterVictor.Set(-1.0);
shooterVictor2.Set(-1.0);
Wait(1.0);
rDrive.MecanumDrive_Cartesian(0.2, 0.0, 0.0);
Wait(0.5);
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
Wait(3.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
loaderArm.Set(true);
Wait(0.5);
loaderArm.Set(false);
Wait(1.5);
shooterVictor.Set(0);
shooterVictor2.Set(0);
*/
while(IsAutonomous()){
Wait(.005);
}
}
void OperatorControl(void){
rDrive.SetSafetyEnabled(true); //turn motor safety timers back on
shooterVictor.SetSafetyEnabled(true);
shooterVictor2.SetSafetyEnabled(true);
DriverStationLCD *ds = DriverStationLCD::GetInstance();
ds->UpdateLCD();
while (IsOperatorControl()){
ds->PrintfLine(DriverStationLCD::kUser_Line1, "Robot Is In TeleOp");
driving(); //runs drive control function
ShooterControl(); //runs shooter control function
LoaderControl(); //runs servo control function
Climbing(); //runs pryamid climbing function
t();
float dfl=efl.Get();
ds->PrintfLine(DriverStationLCD::kUser_Line2, "%f",dfl);
Wait(0.05); //waits 0.02 seconds
}
}
void t(){
if(shooterStick.GetRawButton(2)){
ts.Set(Relay::kForward);
}
else{
ts.Set(Relay::kOff);
}
}
void driving(){
double y = driverStick.GetY(); //variable for forward/backward movement
double x = driverStick.GetX(); //variable for side to side movement
double turn = driverStick2.GetX() ; //variable for turning movement
deadzone(y);
deadzone(x);
deadzone(turn);
mecanum(y, -x, turn);
}
double deadzone(double in){
double f_in=fabs(in);
if(in!=0){
double f_in=fabs(in);
f_in=1.25*f_in;
f_in=f_in-.25;
if(in<0){
f_in=f_in*-1;
}
}
return f_in;
}
void mecanum(float y,float x,float turn){
y=-y;
float mfl,mfr,mbl,mbr;
mfl=y+x+turn;
mfr=y-x-turn;
mbl=y-x+turn;
mbr=x+y-turn;
float max=fabs(mfl);
if(fabs(mfr)>max){
max=fabs(mfr);
}
if(fabs(mbl)>max){
max=fabs(mbl);
}
if(fabs(mbr)>max){
max=fabs(mbr);
}
mfl=mfl/max;
mfr=mfr/max;
mbl=mbl/max;
mbr=mbr/max;
}
void ShooterControl(){ //shooter control function
if(shooterStick.GetRawButton(1)){
shooterVictor.Set(-1.0); //if trigger is pressed, turn on shooter
shooterVictor2.Set(-1.0);
}
else{
shooterVictor.Set(0); //if trigger is pressed, turn on shooter
shooterVictor2.Set(0);
}
}
void LoaderControl(){ //servo control function
if(shooterStick.GetRawButton(3) && shooterStick.GetRawButton(1)){
loaderArm.Set(true); //if middle button is pressed, extend servo
}
else{
loaderArm.Set(false);
}
}
void Climbing(){ //pyramid climbing function, happens automatically
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if( shooterStick.GetRawButton(4)){ //if button 6 pressed on both sticks, starts climb
ds->PrintfLine(DriverStationLCD::kUser_Line6, "Arms Up");
SC1.Set(true);
SC2.Set(true);
}
if(shooterStick.GetRawButton(5)){
ds->PrintfLine(DriverStationLCD::kUser_Line6, "Arms Down");
SC1.Set(false);
SC2.Set(false);
}
}
void Test() {
shooterVictor.Set(0);
shooterVictor2.Set(0);
Compressor *c = new Compressor(3, 2);
c->Start();
Wait(0.05);
}
};
START_ROBOT_CLASS(RobotDemo); //starts the class running