| Arrowhead |
16-02-2013 19:53 |
Wierd Robot Problems
So our team has been experiencing very weird problems with our code. Whenever we try and drive the robot, wheels will spin the wrong way, for seemingly no reason. We think that it may be cause by using the gyro for our field-centric steering, however we are not sure, as this is part of WPILib. Another problem we are having is that several of our functions just dont seem to be running. For inastance, we have a function called speedcontrol, which simply changes the speed of our shooter depending on which button is pressed, then outputs the speed, however the output always remains at 0. Below are 4 copies of the code. One copy is our regular code, one is the code without the deadzone, one is without field-centric steering, and finally, one is without either the deadzone or the field-centric steering. Help us CD, you are our only hope.
Code:
/*
Minotaur FRC 1369
2013 Robot Code
Lead Cracker: Graham Peterson
Lead Lead: Daniel Brown
*Insert Important Stuff Here*
*/
#include "WPILib.h"
#include <math.h>
class RobotDemo : public SimpleRobot
{
Victor laquanda; //front left motor:V1
Victor anabel; //back left motor:V2
Victor rotciv; //front right motor:V3
Victor shanaynay; //back right motor:V4
RobotDrive rDrive; //declare robot drive
Victor shooterVictor; //declare victor for shooter
Victor shooterVictor2;
Victor shooterVictor3;
Solenoid loaderArm; //declare servo for loader arm
Joystick driverStick; //declares all 3 joysticks
Joystick driverStick2;
Joystick shooterStick;
Victor climbingArm1; //declare victors for climbing arms
Victor climbingArm2;
Solenoid SC1; //declares solenoids for climbing arm
Solenoid SC2;
DigitalInput Disk1; //declares limit switches for hopper
DigitalInput Disk2;
DigitalInput Disk3;
Gyro g1;
public:
RobotDemo(void):
laquanda(1), //assign drive motors PWM ports
anabel(2),
rotciv(3),
shanaynay(4),
rDrive(laquanda, anabel, rotciv, shanaynay), //assigns robot drive talons theiw PWM ports
shooterVictor(6), //assigns victor its PWM port
shooterVictor2(7),
shooterVictor3(8),
loaderArm(3), //assign servo its PWM port
driverStick(1), //assign joysticks their USB ports
driverStick2(2),
shooterStick(3),
climbingArm1(8), //assigns climbing victors PWM ports
climbingArm2(9),
SC1(1), //assigns climbing pneumatics digital output ports
SC2(2),
Disk1(5), //assigns limit switches digital input ports
Disk2(6),
Disk3(7),
g1(1)
{
Compressor *c = new Compressor(4, 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);
shooterVictor3.SetExpiration(0.1);
shooterVictor3.SetSafetyEnabled(true);
climbingArm1.SetExpiration(0.1);
climbingArm1.SetSafetyEnabled(false);
climbingArm2.SetExpiration(0.1);
climbingArm2.SetSafetyEnabled(false);
}
void Autonomous(void)
{
g1.Reset();
DriverStationLCD *ds = DriverStationLCD::GetInstance();
ds->Clear();
double AutoTime = 0; //used to measure time during autonomous
rDrive.SetSafetyEnabled(false); //turn of motor saftey timers during autonomous for convience
shooterVictor.SetSafetyEnabled(false);
shooterVictor2.SetSafetyEnabled(false);
shooterVictor3.SetSafetyEnabled(false);
ds->PrintfLine(DriverStationLCD::kUser_Line1, "Program Started Successfully!");
ds->PrintfLine(DriverStationLCD::kUser_Line2, "Robot Is in Autonomous");
ds->UpdateLCD();
while(IsAutonomous()){
//top autonomous
if(AutoTime>=0 && AutoTime<0.5){
rDrive.MecanumDrive_Cartesian(0.0, 0.5, 0.0);
}
if(AutoTime>=0.5 && AutoTime<1.0){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, -0.2);//-0.2 for right start, 0.2 for left start
}
if(AutoTime>=1.0 && AutoTime<1.5){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
shooterVictor.Set(1.0);
shooterVictor2.Set(1.0);
shooterVictor3.Set(1.0);
loaderArm.Set(true);
}
if(AutoTime>=1.5 && AutoTime<2.0){
loaderArm.Set(false);
}
if(AutoTime>=2.0 && AutoTime<2.5){
loaderArm.Set(true);
}
if(AutoTime>=2.5 && AutoTime<3.0){
loaderArm.Set(false);
}
if(AutoTime>=3.0 && AutoTime<3.5){
loaderArm.Set(true);
}
if(AutoTime>=3.5 && AutoTime<4.0){
loaderArm.Set(false);
shooterVictor.Set(0.0);
shooterVictor2.Set(0.0);
shooterVictor3.Set(0.0);
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.2); // opposite of above turn
}
if(AutoTime>=4.0 && AutoTime<5.0){
rDrive.MecanumDrive_Cartesian(1.0, 0.0, 0.0); //1.0 for right start, -1.0 for left start
}
if(AutoTime>=5.0){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0); //1.0 for right start, -1.0 for left start
}
//start at back autonomous
/*
if(AutoTime>=0 && AutoTime<0.5){
rDrive.MecanumDrive_Cartesian(0.5, 0.0, 0.0); //0.5 for right start, -0.5 for left start
}
if(AutoTime>=0.5 && AutoTime<1.0){
rDrive.MecanumDrive_Cartesian(0.0, 1.0, 0.0);
}
if(AutoTime>=1.0 && AutoTime<1.5){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, -0.2);//-0.2 for right start, 0.2 for left start
}
if(AutoTime>=1.5 && AutoTime<2.0){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
shooterVictor.Set(1.0);
shooterVictor2.Set(1.0);
shooterVictor3.Set(1.0);
loaderArm.Set(true);
}
if(AutoTime>=2.0 && AutoTime<2.5){
loaderArm.Set(false);
}
if(AutoTime>=2.5 && AutoTime<3.0){
loaderArm.Set(true);
}
if(AutoTime>=3.0 && AutoTime<3.5){
loaderArm.Set(false);
}
if(AutoTime>=3.5 && AutoTime<4.0){
loaderArm.Set(true);
}
if(AutoTime>=4.0 && AutoTime<4.5){
loaderArm.Set(false);
shooterVictor.Set(0.0);
shooterVictor2.Set(0.0);
shooterVictor3.Set(0.0);
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.2); // opposite of above turn
}
if(AutoTime>=4.5 && AutoTime<5.0){
rDrive.MecanumDrive_Cartesian(1.0, 0.0, 0.0); //1.0 for right start, -1.0 for left start
}
if(AutoTime>=5.0){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0); //1.0 for right start, -1.0 for left start
}
*/
AutoTime=AutoTime+0.005;
Wait(0.005);
}
}
void OperatorControl(void){
g1.Reset();
rDrive.SetSafetyEnabled(true); //turn motor safety timers back on
shooterVictor.SetSafetyEnabled(true);
shooterVictor2.SetSafetyEnabled(true);
shooterVictor3.SetSafetyEnabled(true);
bool gyroe = true;
float shootspeed=0.25;
DriverStationLCD *ds = DriverStationLCD::GetInstance();
ds->PrintfLine(DriverStationLCD::kUser_Line6, "Pinkie Pie Is Best Pony");
ds->UpdateLCD();
while (IsOperatorControl()){
ds->PrintfLine(DriverStationLCD::kUser_Line2, "Robot Is In TeleOp");
gyroe=gyroc(gyroe);
shootspeed=speedcontrol(shootspeed);
gc();
driving(gyroe); //runs drive control function
ShooterControl(shootspeed); //runs shooter control function
LoaderControl(); //runs servo control function
Climbing(); //runs pryamid climbing function
//DiskCount(); //runs diskcount function
ds->UpdateLCD();
Wait(0.005); //waits 0.02 seconds
}
}
void gc(){
if(driverStick.GetRawButton(2)){
g1.Reset();
}
}
float speedcontrol(float s){
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if (shooterStick.GetRawButton(6)){
s=0.25;
}
else if (shooterStick.GetRawButton(7)){
s=0.5;
}
else if (shooterStick.GetRawButton(10)){
s=0.75;
}
else if (shooterStick.GetRawButton(11)){
s=1.0;
}
ds->PrintfLine(DriverStationLCD::kUser_Line1, "Shooter Speed: %d", s);
return s;
}
bool gyroc(bool gyroe){
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if (driverStick.GetRawButton(4)){
}
ds->PrintfLine(DriverStationLCD::kUser_Line4, "Gyro On: %d", gyroe);
ds->PrintfLine(DriverStationLCD::kUser_Line5, " %d", g1.GetAngle());
ds->UpdateLCD();
return gyroe;
}
void driving(bool gyroe){
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
double deadzone = 0.2; //variable for amount of deadzone
if(fabs(y)>deadzone) {
float yo = y;
y=fabs(y);
y=1-y;
y=y*.1;
y=y*2.5;
y=yo-y;
if(driverStick.GetY()<0){
y=-y;
}
}
if(fabs(x)>deadzone) {
float xo = x;
x=fabs(x);
x=1-x;
x=x*.1;
x=x*2.5;
x=xo-x;
if(driverStick.GetX()<0){
x=-x;
}
}
if(fabs(turn)>deadzone) {
float turno = turn;
turn=fabs(turn);
turn=1-turn;
turn=turn*.1;
turn=turn*2.5;
turn=turno-turn;
if(driverStick2.GetX()<0){
turn=-turn;
}
}
if (gyroe=true){
rDrive.MecanumDrive_Cartesian(x, y, -turn, g1.GetAngle());
}
else{
rDrive.MecanumDrive_Cartesian(x, y, -turn);
}
}
void ShooterControl(float shootspeed){ //shooter control function
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if(shooterStick.GetRawButton(1)){
shooterVictor.Set(shootspeed); //if trigger is pressed, turn on shooter
shooterVictor2.Set(shootspeed);
shooterVictor3.Set(shootspeed);
ds->PrintfLine(DriverStationLCD::kUser_Line3, "Shooter is On");
}
else{
shooterVictor.Set(0.0); //if trigger isnt pressed, turn off shooter
shooterVictor2.Set(0.0);
shooterVictor3.Set(0.0);
ds->PrintfLine(DriverStationLCD::kUser_Line3, "Shooter is Off");
}
}
void LoaderControl(){ //servo control function
if(shooterStick.GetRawButton(3)){
loaderArm.Set(true); //if middle button is pressed, extend servo
}
else{
loaderArm.Set(false); //if middle button isnt pressed, retract servo
}
}
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_Line4, "Arms Up");
SC1.Set(true);
SC2.Set(true);
}
if(shooterStick.GetRawButton(5)){
ds->PrintfLine(DriverStationLCD::kUser_Line4, "Arms Down");
SC1.Set(false);
SC2.Set(false);
}
}
/*
void DiskCount(){ //function that counts disk in the hopper and displays it
DriverStationLCD *ds = DriverStationLCD::GetInstance();
int disknum = 0; //variable that counts disks in hopper
if(Disk1.Get()){
disknum=disknum+1; //disk has activated a limit switch, add 1 to # of disks
}
if(Disk2.Get()){
disknum=disknum+1;
}
if(Disk3.Get()){
disknum=disknum+1;
}
ds->PrintfLine(DriverStationLCD::kUser_Line5, "Disk's in Hopper: %d", disknum); //print out the number of disks in hopper
}
*/
};
START_ROBOT_CLASS(RobotDemo); //starts the class running
Code:
/*
Minotaur FRC 1369
2013 Robot Code
Lead Cracker: Graham Peterson
Lead Lead: Daniel Brown
*Insert Important Stuff Here*
*/
//no deadzone
#include "WPILib.h"
#include <math.h>
class RobotDemo : public SimpleRobot
{
Victor laquanda; //front left motor:V1
Victor anabel; //back left motor:V2
Victor rotciv; //front right motor:V3
Victor shanaynay; //back right motor:V4
RobotDrive rDrive; //declare robot drive
Victor shooterVictor; //declare victor for shooter
Victor shooterVictor2;
Victor shooterVictor3;
Solenoid loaderArm; //declare servo for loader arm
Joystick driverStick; //declares all 3 joysticks
Joystick driverStick2;
Joystick shooterStick;
Victor climbingArm1; //declare victors for climbing arms
Victor climbingArm2;
Solenoid SC1; //declares solenoids for climbing arm
Solenoid SC2;
DigitalInput Disk1; //declares limit switches for hopper
DigitalInput Disk2;
DigitalInput Disk3;
Gyro g1;
public:
RobotDemo(void):
laquanda(1), //assign drive motors PWM ports
anabel(2),
rotciv(3),
shanaynay(4),
rDrive(laquanda, anabel, rotciv, shanaynay), //assigns robot drive talons theiw PWM ports
shooterVictor(6), //assigns victor its PWM port
shooterVictor2(7),
shooterVictor3(8),
loaderArm(3), //assign servo its PWM port
driverStick(1), //assign joysticks their USB ports
driverStick2(2),
shooterStick(3),
climbingArm1(8), //assigns climbing victors PWM ports
climbingArm2(9),
SC1(1), //assigns climbing pneumatics digital output ports
SC2(2),
Disk1(5), //assigns limit switches digital input ports
Disk2(6),
Disk3(7),
g1(1)
{
Compressor *c = new Compressor(4, 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);
shooterVictor3.SetExpiration(0.1);
shooterVictor3.SetSafetyEnabled(true);
climbingArm1.SetExpiration(0.1);
climbingArm1.SetSafetyEnabled(false);
climbingArm2.SetExpiration(0.1);
climbingArm2.SetSafetyEnabled(false);
}
void Autonomous(void)
{
g1.Reset();
DriverStationLCD *ds = DriverStationLCD::GetInstance();
ds->Clear();
double AutoTime = 0; //used to measure time during autonomous
rDrive.SetSafetyEnabled(false); //turn of motor saftey timers during autonomous for convience
shooterVictor.SetSafetyEnabled(false);
shooterVictor2.SetSafetyEnabled(false);
shooterVictor3.SetSafetyEnabled(false);
ds->PrintfLine(DriverStationLCD::kUser_Line1, "Program Started Successfully!");
ds->PrintfLine(DriverStationLCD::kUser_Line2, "Robot Is in Autonomous");
ds->UpdateLCD();
while(IsAutonomous()){
//top autonomous
if(AutoTime>=0 && AutoTime<0.5){
rDrive.MecanumDrive_Cartesian(0.0, 0.5, 0.0);
}
if(AutoTime>=0.5 && AutoTime<1.0){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, -0.2);//-0.2 for right start, 0.2 for left start
}
if(AutoTime>=1.0 && AutoTime<1.5){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
shooterVictor.Set(1.0);
shooterVictor2.Set(1.0);
shooterVictor3.Set(1.0);
loaderArm.Set(true);
}
if(AutoTime>=1.5 && AutoTime<2.0){
loaderArm.Set(false);
}
if(AutoTime>=2.0 && AutoTime<2.5){
loaderArm.Set(true);
}
if(AutoTime>=2.5 && AutoTime<3.0){
loaderArm.Set(false);
}
if(AutoTime>=3.0 && AutoTime<3.5){
loaderArm.Set(true);
}
if(AutoTime>=3.5 && AutoTime<4.0){
loaderArm.Set(false);
shooterVictor.Set(0.0);
shooterVictor2.Set(0.0);
shooterVictor3.Set(0.0);
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.2); // opposite of above turn
}
if(AutoTime>=4.0 && AutoTime<5.0){
rDrive.MecanumDrive_Cartesian(1.0, 0.0, 0.0); //1.0 for right start, -1.0 for left start
}
if(AutoTime>=5.0){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0); //1.0 for right start, -1.0 for left start
}
//start at back autonomous
/*
if(AutoTime>=0 && AutoTime<0.5){
rDrive.MecanumDrive_Cartesian(0.5, 0.0, 0.0); //0.5 for right start, -0.5 for left start
}
if(AutoTime>=0.5 && AutoTime<1.0){
rDrive.MecanumDrive_Cartesian(0.0, 1.0, 0.0);
}
if(AutoTime>=1.0 && AutoTime<1.5){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, -0.2);//-0.2 for right start, 0.2 for left start
}
if(AutoTime>=1.5 && AutoTime<2.0){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
shooterVictor.Set(1.0);
shooterVictor2.Set(1.0);
shooterVictor3.Set(1.0);
loaderArm.Set(true);
}
if(AutoTime>=2.0 && AutoTime<2.5){
loaderArm.Set(false);
}
if(AutoTime>=2.5 && AutoTime<3.0){
loaderArm.Set(true);
}
if(AutoTime>=3.0 && AutoTime<3.5){
loaderArm.Set(false);
}
if(AutoTime>=3.5 && AutoTime<4.0){
loaderArm.Set(true);
}
if(AutoTime>=4.0 && AutoTime<4.5){
loaderArm.Set(false);
shooterVictor.Set(0.0);
shooterVictor2.Set(0.0);
shooterVictor3.Set(0.0);
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.2); // opposite of above turn
}
if(AutoTime>=4.5 && AutoTime<5.0){
rDrive.MecanumDrive_Cartesian(1.0, 0.0, 0.0); //1.0 for right start, -1.0 for left start
}
if(AutoTime>=5.0){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0); //1.0 for right start, -1.0 for left start
}
*/
AutoTime=AutoTime+0.005;
Wait(0.005);
}
}
void OperatorControl(void){
g1.Reset();
rDrive.SetSafetyEnabled(true); //turn motor safety timers back on
shooterVictor.SetSafetyEnabled(true);
shooterVictor2.SetSafetyEnabled(true);
shooterVictor3.SetSafetyEnabled(true);
bool gyroe = true;
float shootspeed=0.25;
DriverStationLCD *ds = DriverStationLCD::GetInstance();
ds->PrintfLine(DriverStationLCD::kUser_Line6, "Pinkie Pie Is Best Pony");
ds->UpdateLCD();
while (IsOperatorControl()){
ds->PrintfLine(DriverStationLCD::kUser_Line2, "Robot Is In TeleOp");
gyroe=gyroc(gyroe);
shootspeed=speedcontrol(shootspeed);
gc();
driving(gyroe); //runs drive control function
ShooterControl(shootspeed); //runs shooter control function
LoaderControl(); //runs servo control function
Climbing(); //runs pryamid climbing function
//DiskCount(); //runs diskcount function
ds->UpdateLCD();
Wait(0.005); //waits 0.02 seconds
}
}
void gc(){
if(driverStick.GetRawButton(2)){
g1.Reset();
}
}
float speedcontrol(float s){
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if (shooterStick.GetRawButton(6)){
s=0.25;
}
else if (shooterStick.GetRawButton(7)){
s=0.5;
}
else if (shooterStick.GetRawButton(10)){
s=0.75;
}
else if (shooterStick.GetRawButton(11)){
s=1.0;
}
ds->PrintfLine(DriverStationLCD::kUser_Line1, "Shooter Speed: %d", s);
return s;
}
bool gyroc(bool gyroe){
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if (driverStick.GetRawButton(3)){
if(gyroe==true){
gyroe=false;
}
else{
gyroe=true;
}
}
ds->PrintfLine(DriverStationLCD::kUser_Line4, "Gyro On: %d", gyroe);
ds->PrintfLine(DriverStationLCD::kUser_Line5, " %d", g1.GetAngle());
ds->UpdateLCD();
return gyroe;
}
void driving(bool gyroe){
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
if (gyroe=true){
rDrive.MecanumDrive_Cartesian(x, y, -turn, g1.GetAngle());
}
else{
rDrive.MecanumDrive_Cartesian(x, y, -turn);
}
}
void ShooterControl(float shootspeed){ //shooter control function
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if(shooterStick.GetRawButton(1)){
shooterVictor.Set(shootspeed); //if trigger is pressed, turn on shooter
shooterVictor2.Set(shootspeed);
shooterVictor3.Set(shootspeed);
ds->PrintfLine(DriverStationLCD::kUser_Line3, "Shooter is On");
}
else{
shooterVictor.Set(0.0); //if trigger isnt pressed, turn off shooter
shooterVictor2.Set(0.0);
shooterVictor3.Set(0.0);
ds->PrintfLine(DriverStationLCD::kUser_Line3, "Shooter is Off");
}
}
void LoaderControl(){ //servo control function
if(shooterStick.GetRawButton(3)){
loaderArm.Set(true); //if middle button is pressed, extend servo
}
else{
loaderArm.Set(false); //if middle button isnt pressed, retract servo
}
}
void Climbing(){ //pyramid climbing function, happens automatically
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if( driverStick.GetRawButton(4)){ //if button 6 pressed on both sticks, starts climb
ds->PrintfLine(DriverStationLCD::kUser_Line4, "Arms Up");
SC1.Set(true);
SC2.Set(true);
}
if(driverStick.GetRawButton(5)){
ds->PrintfLine(DriverStationLCD::kUser_Line4, "Arms Down");
SC1.Set(false);
SC2.Set(false);
}
}
/*
void DiskCount(){ //function that counts disk in the hopper and displays it
DriverStationLCD *ds = DriverStationLCD::GetInstance();
int disknum = 0; //variable that counts disks in hopper
if(Disk1.Get()){
disknum=disknum+1; //disk has activated a limit switch, add 1 to # of disks
}
if(Disk2.Get()){
disknum=disknum+1;
}
if(Disk3.Get()){
disknum=disknum+1;
}
ds->PrintfLine(DriverStationLCD::kUser_Line5, "Disk's in Hopper: %d", disknum); //print out the number of disks in hopper
}
*/
};
START_ROBOT_CLASS(RobotDemo); //starts the class running
Code:
/*
Minotaur FRC 1369
2013 Robot Code
Lead Cracker: Graham Peterson
Lead Lead: Daniel Brown
*Insert Important Stuff Here*
*/
//no field centric
#include "WPILib.h"
#include <math.h>
class RobotDemo : public SimpleRobot
{
Victor laquanda; //front left motor:V1
Victor anabel; //back left motor:V2
Victor rotciv; //front right motor:V3
Victor shanaynay; //back right motor:V4
RobotDrive rDrive; //declare robot drive
Victor shooterVictor; //declare victor for shooter
Victor shooterVictor2;
Victor shooterVictor3;
Solenoid loaderArm; //declare servo for loader arm
Joystick driverStick; //declares all 3 joysticks
Joystick driverStick2;
Joystick shooterStick;
Victor climbingArm1; //declare victors for climbing arms
Victor climbingArm2;
Solenoid SC1; //declares solenoids for climbing arm
Solenoid SC2;
DigitalInput Disk1; //declares limit switches for hopper
DigitalInput Disk2;
DigitalInput Disk3;
Gyro g1;
public:
RobotDemo(void):
laquanda(1), //assign drive motors PWM ports
anabel(2),
rotciv(3),
shanaynay(4),
rDrive(laquanda, anabel, rotciv, shanaynay), //assigns robot drive talons theiw PWM ports
shooterVictor(6), //assigns victor its PWM port
shooterVictor2(7),
shooterVictor3(8),
loaderArm(3), //assign servo its PWM port
driverStick(1), //assign joysticks their USB ports
driverStick2(2),
shooterStick(3),
climbingArm1(8), //assigns climbing victors PWM ports
climbingArm2(9),
SC1(1), //assigns climbing pneumatics digital output ports
SC2(2),
Disk1(5), //assigns limit switches digital input ports
Disk2(6),
Disk3(7),
g1(1)
{
Compressor *c = new Compressor(4, 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);
shooterVictor3.SetExpiration(0.1);
shooterVictor3.SetSafetyEnabled(true);
climbingArm1.SetExpiration(0.1);
climbingArm1.SetSafetyEnabled(false);
climbingArm2.SetExpiration(0.1);
climbingArm2.SetSafetyEnabled(false);
}
void Autonomous(void)
{
g1.Reset();
DriverStationLCD *ds = DriverStationLCD::GetInstance();
ds->Clear();
double AutoTime = 0; //used to measure time during autonomous
rDrive.SetSafetyEnabled(false); //turn of motor saftey timers during autonomous for convience
shooterVictor.SetSafetyEnabled(false);
shooterVictor2.SetSafetyEnabled(false);
shooterVictor3.SetSafetyEnabled(false);
ds->PrintfLine(DriverStationLCD::kUser_Line1, "Program Started Successfully!");
ds->PrintfLine(DriverStationLCD::kUser_Line2, "Robot Is in Autonomous");
ds->UpdateLCD();
while(IsAutonomous()){
//top autonomous
if(AutoTime>=0 && AutoTime<0.5){
rDrive.MecanumDrive_Cartesian(0.0, 0.5, 0.0);
}
if(AutoTime>=0.5 && AutoTime<1.0){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, -0.2);//-0.2 for right start, 0.2 for left start
}
if(AutoTime>=1.0 && AutoTime<1.5){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
shooterVictor.Set(1.0);
shooterVictor2.Set(1.0);
shooterVictor3.Set(1.0);
loaderArm.Set(true);
}
if(AutoTime>=1.5 && AutoTime<2.0){
loaderArm.Set(false);
}
if(AutoTime>=2.0 && AutoTime<2.5){
loaderArm.Set(true);
}
if(AutoTime>=2.5 && AutoTime<3.0){
loaderArm.Set(false);
}
if(AutoTime>=3.0 && AutoTime<3.5){
loaderArm.Set(true);
}
if(AutoTime>=3.5 && AutoTime<4.0){
loaderArm.Set(false);
shooterVictor.Set(0.0);
shooterVictor2.Set(0.0);
shooterVictor3.Set(0.0);
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.2); // opposite of above turn
}
if(AutoTime>=4.0 && AutoTime<5.0){
rDrive.MecanumDrive_Cartesian(1.0, 0.0, 0.0); //1.0 for right start, -1.0 for left start
}
if(AutoTime>=5.0){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0); //1.0 for right start, -1.0 for left start
}
//start at back autonomous
/*
if(AutoTime>=0 && AutoTime<0.5){
rDrive.MecanumDrive_Cartesian(0.5, 0.0, 0.0); //0.5 for right start, -0.5 for left start
}
if(AutoTime>=0.5 && AutoTime<1.0){
rDrive.MecanumDrive_Cartesian(0.0, 1.0, 0.0);
}
if(AutoTime>=1.0 && AutoTime<1.5){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, -0.2);//-0.2 for right start, 0.2 for left start
}
if(AutoTime>=1.5 && AutoTime<2.0){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
shooterVictor.Set(1.0);
shooterVictor2.Set(1.0);
shooterVictor3.Set(1.0);
loaderArm.Set(true);
}
if(AutoTime>=2.0 && AutoTime<2.5){
loaderArm.Set(false);
}
if(AutoTime>=2.5 && AutoTime<3.0){
loaderArm.Set(true);
}
if(AutoTime>=3.0 && AutoTime<3.5){
loaderArm.Set(false);
}
if(AutoTime>=3.5 && AutoTime<4.0){
loaderArm.Set(true);
}
if(AutoTime>=4.0 && AutoTime<4.5){
loaderArm.Set(false);
shooterVictor.Set(0.0);
shooterVictor2.Set(0.0);
shooterVictor3.Set(0.0);
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.2); // opposite of above turn
}
if(AutoTime>=4.5 && AutoTime<5.0){
rDrive.MecanumDrive_Cartesian(1.0, 0.0, 0.0); //1.0 for right start, -1.0 for left start
}
if(AutoTime>=5.0){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0); //1.0 for right start, -1.0 for left start
}
*/
AutoTime=AutoTime+0.005;
Wait(0.005);
}
}
void OperatorControl(void){
g1.Reset();
rDrive.SetSafetyEnabled(true); //turn motor safety timers back on
shooterVictor.SetSafetyEnabled(true);
shooterVictor2.SetSafetyEnabled(true);
shooterVictor3.SetSafetyEnabled(true);
bool gyroe = true;
float shootspeed=0.25;
DriverStationLCD *ds = DriverStationLCD::GetInstance();
ds->PrintfLine(DriverStationLCD::kUser_Line6, "Pinkie Pie Is Best Pony");
ds->UpdateLCD();
while (IsOperatorControl()){
ds->PrintfLine(DriverStationLCD::kUser_Line2, "Robot Is In TeleOp");
shootspeed=speedcontrol(shootspeed);
driving(gyroe); //runs drive control function
ShooterControl(shootspeed); //runs shooter control function
LoaderControl(); //runs servo control function
Climbing(); //runs pryamid climbing function
//DiskCount(); //runs diskcount function
ds->UpdateLCD();
Wait(0.005); //waits 0.02 seconds
}
}
float speedcontrol(float s){
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if (shooterStick.GetRawButton(6)){
s=0.25;
}
else if (shooterStick.GetRawButton(7)){
s=0.5;
}
else if (shooterStick.GetRawButton(10)){
s=0.75;
}
else if (shooterStick.GetRawButton(11)){
s=1.0;
}
ds->PrintfLine(DriverStationLCD::kUser_Line1, "Shooter Speed: %d", s);
return s;
}
void driving(bool gyroe){
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
double deadzone = 0.2; //variable for amount of deadzone
if(fabs(y)>deadzone) {
float yo = y;
y=fabs(y);
y=1-y;
y=y*.1;
y=y*2.5;
y=yo-y;
if(driverStick.GetY()<0){
y=-y;
}
}
if(fabs(x)>deadzone) {
float xo = x;
x=fabs(x);
x=1-x;
x=x*.1;
x=x*2.5;
x=xo-x;
if(driverStick.GetX()<0){
x=-x;
}
}
if(fabs(turn)>deadzone) {
float turno = turn;
turn=fabs(turn);
turn=1-turn;
turn=turn*.1;
turn=turn*2.5;
turn=turno-turn;
if(driverStick2.GetX()<0){
turn=-turn;
}
}
rDrive.MecanumDrive_Cartesian(x, y, -turn);
}
void ShooterControl(float shootspeed){ //shooter control function
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if(shooterStick.GetRawButton(1)){
shooterVictor.Set(shootspeed); //if trigger is pressed, turn on shooter
shooterVictor2.Set(shootspeed);
shooterVictor3.Set(shootspeed);
ds->PrintfLine(DriverStationLCD::kUser_Line3, "Shooter is On");
}
else{
shooterVictor.Set(0.0); //if trigger isnt pressed, turn off shooter
shooterVictor2.Set(0.0);
shooterVictor3.Set(0.0);
ds->PrintfLine(DriverStationLCD::kUser_Line3, "Shooter is Off");
}
}
void LoaderControl(){ //servo control function
if(shooterStick.GetRawButton(3)){
loaderArm.Set(true); //if middle button is pressed, extend servo
}
else{
loaderArm.Set(false); //if middle button isnt pressed, retract servo
}
}
void Climbing(){ //pyramid climbing function, happens automatically
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if( driverStick.GetRawButton(4)){ //if button 6 pressed on both sticks, starts climb
ds->PrintfLine(DriverStationLCD::kUser_Line4, "Arms Up");
SC1.Set(true);
SC2.Set(true);
}
if(driverStick.GetRawButton(5)){
ds->PrintfLine(DriverStationLCD::kUser_Line4, "Arms Down");
SC1.Set(false);
SC2.Set(false);
}
}
/*
void DiskCount(){ //function that counts disk in the hopper and displays it
DriverStationLCD *ds = DriverStationLCD::GetInstance();
int disknum = 0; //variable that counts disks in hopper
if(Disk1.Get()){
disknum=disknum+1; //disk has activated a limit switch, add 1 to # of disks
}
if(Disk2.Get()){
disknum=disknum+1;
}
if(Disk3.Get()){
disknum=disknum+1;
}
ds->PrintfLine(DriverStationLCD::kUser_Line5, "Disk's in Hopper: %d", disknum); //print out the number of disks in hopper
}
*/
};
START_ROBOT_CLASS(RobotDemo); //starts the class running
Code:
/*
Minotaur FRC 1369
2013 Robot Code
Lead Cracker: Graham Peterson
Lead Lead: Daniel Brown
*Insert Important Stuff Here*
*/
//nothing
#include "WPILib.h"
#include <math.h>
class RobotDemo : public SimpleRobot
{
Victor laquanda; //front left motor:V1
Victor anabel; //back left motor:V2
Victor rotciv; //front right motor:V3
Victor shanaynay; //back right motor:V4
RobotDrive rDrive; //declare robot drive
Victor shooterVictor; //declare victor for shooter
Victor shooterVictor2;
Victor shooterVictor3;
Solenoid loaderArm; //declare servo for loader arm
Joystick driverStick; //declares all 3 joysticks
Joystick driverStick2;
Joystick shooterStick;
Victor climbingArm1; //declare victors for climbing arms
Victor climbingArm2;
Solenoid SC1; //declares solenoids for climbing arm
Solenoid SC2;
DigitalInput Disk1; //declares limit switches for hopper
DigitalInput Disk2;
DigitalInput Disk3;
Gyro g1;
public:
RobotDemo(void):
laquanda(1), //assign drive motors PWM ports
anabel(2),
rotciv(3),
shanaynay(4),
rDrive(laquanda, anabel, rotciv, shanaynay), //assigns robot drive talons theiw PWM ports
shooterVictor(6), //assigns victor its PWM port
shooterVictor2(7),
shooterVictor3(8),
loaderArm(3), //assign servo its PWM port
driverStick(1), //assign joysticks their USB ports
driverStick2(2),
shooterStick(3),
climbingArm1(8), //assigns climbing victors PWM ports
climbingArm2(9),
SC1(1), //assigns climbing pneumatics digital output ports
SC2(2),
Disk1(5), //assigns limit switches digital input ports
Disk2(6),
Disk3(7),
g1(1)
{
Compressor *c = new Compressor(4, 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);
shooterVictor3.SetExpiration(0.1);
shooterVictor3.SetSafetyEnabled(true);
climbingArm1.SetExpiration(0.1);
climbingArm1.SetSafetyEnabled(false);
climbingArm2.SetExpiration(0.1);
climbingArm2.SetSafetyEnabled(false);
}
void Autonomous(void)
{
g1.Reset();
DriverStationLCD *ds = DriverStationLCD::GetInstance();
ds->Clear();
double AutoTime = 0; //used to measure time during autonomous
rDrive.SetSafetyEnabled(false); //turn of motor saftey timers during autonomous for convience
shooterVictor.SetSafetyEnabled(false);
shooterVictor2.SetSafetyEnabled(false);
shooterVictor3.SetSafetyEnabled(false);
ds->PrintfLine(DriverStationLCD::kUser_Line1, "Program Started Successfully!");
ds->PrintfLine(DriverStationLCD::kUser_Line2, "Robot Is in Autonomous");
ds->UpdateLCD();
while(IsAutonomous()){
//top autonomous
if(AutoTime>=0 && AutoTime<0.5){
rDrive.MecanumDrive_Cartesian(0.0, 0.5, 0.0);
}
if(AutoTime>=0.5 && AutoTime<1.0){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, -0.2);//-0.2 for right start, 0.2 for left start
}
if(AutoTime>=1.0 && AutoTime<1.5){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
shooterVictor.Set(1.0);
shooterVictor2.Set(1.0);
shooterVictor3.Set(1.0);
loaderArm.Set(true);
}
if(AutoTime>=1.5 && AutoTime<2.0){
loaderArm.Set(false);
}
if(AutoTime>=2.0 && AutoTime<2.5){
loaderArm.Set(true);
}
if(AutoTime>=2.5 && AutoTime<3.0){
loaderArm.Set(false);
}
if(AutoTime>=3.0 && AutoTime<3.5){
loaderArm.Set(true);
}
if(AutoTime>=3.5 && AutoTime<4.0){
loaderArm.Set(false);
shooterVictor.Set(0.0);
shooterVictor2.Set(0.0);
shooterVictor3.Set(0.0);
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.2); // opposite of above turn
}
if(AutoTime>=4.0 && AutoTime<5.0){
rDrive.MecanumDrive_Cartesian(1.0, 0.0, 0.0); //1.0 for right start, -1.0 for left start
}
if(AutoTime>=5.0){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0); //1.0 for right start, -1.0 for left start
}
//start at back autonomous
/*
if(AutoTime>=0 && AutoTime<0.5){
rDrive.MecanumDrive_Cartesian(0.5, 0.0, 0.0); //0.5 for right start, -0.5 for left start
}
if(AutoTime>=0.5 && AutoTime<1.0){
rDrive.MecanumDrive_Cartesian(0.0, 1.0, 0.0);
}
if(AutoTime>=1.0 && AutoTime<1.5){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, -0.2);//-0.2 for right start, 0.2 for left start
}
if(AutoTime>=1.5 && AutoTime<2.0){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
shooterVictor.Set(1.0);
shooterVictor2.Set(1.0);
shooterVictor3.Set(1.0);
loaderArm.Set(true);
}
if(AutoTime>=2.0 && AutoTime<2.5){
loaderArm.Set(false);
}
if(AutoTime>=2.5 && AutoTime<3.0){
loaderArm.Set(true);
}
if(AutoTime>=3.0 && AutoTime<3.5){
loaderArm.Set(false);
}
if(AutoTime>=3.5 && AutoTime<4.0){
loaderArm.Set(true);
}
if(AutoTime>=4.0 && AutoTime<4.5){
loaderArm.Set(false);
shooterVictor.Set(0.0);
shooterVictor2.Set(0.0);
shooterVictor3.Set(0.0);
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.2); // opposite of above turn
}
if(AutoTime>=4.5 && AutoTime<5.0){
rDrive.MecanumDrive_Cartesian(1.0, 0.0, 0.0); //1.0 for right start, -1.0 for left start
}
if(AutoTime>=5.0){
rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0); //1.0 for right start, -1.0 for left start
}
*/
AutoTime=AutoTime+0.005;
Wait(0.005);
}
}
void OperatorControl(void){
g1.Reset();
rDrive.SetSafetyEnabled(true); //turn motor safety timers back on
shooterVictor.SetSafetyEnabled(true);
shooterVictor2.SetSafetyEnabled(true);
shooterVictor3.SetSafetyEnabled(true);
bool gyroe = true;
float shootspeed=0.25;
DriverStationLCD *ds = DriverStationLCD::GetInstance();
ds->PrintfLine(DriverStationLCD::kUser_Line6, "Pinkie Pie Is Best Pony");
ds->UpdateLCD();
while (IsOperatorControl()){
ds->PrintfLine(DriverStationLCD::kUser_Line2, "Robot Is In TeleOp");
shootspeed=speedcontrol(shootspeed);
driving(gyroe); //runs drive control function
ShooterControl(shootspeed); //runs shooter control function
LoaderControl(); //runs servo control function
Climbing(); //runs pryamid climbing function
//DiskCount(); //runs diskcount function
ds->UpdateLCD();
Wait(0.005); //waits 0.02 seconds
}
}
float speedcontrol(float s){
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if (shooterStick.GetRawButton(6)){
s=0.25;
}
else if (shooterStick.GetRawButton(7)){
s=0.5;
}
else if (shooterStick.GetRawButton(10)){
s=0.75;
}
else if (shooterStick.GetRawButton(11)){
s=1.0;
}
ds->PrintfLine(DriverStationLCD::kUser_Line1, "Shooter Speed: %d", s);
return s;
}
void driving(bool gyroe){
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
rDrive.MecanumDrive_Cartesian(x, y, -turn);
}
void ShooterControl(float shootspeed){ //shooter control function
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if(shooterStick.GetRawButton(1)){
shooterVictor.Set(shootspeed); //if trigger is pressed, turn on shooter
shooterVictor2.Set(shootspeed);
shooterVictor3.Set(shootspeed);
ds->PrintfLine(DriverStationLCD::kUser_Line3, "Shooter is On");
}
else{
shooterVictor.Set(0.0); //if trigger isnt pressed, turn off shooter
shooterVictor2.Set(0.0);
shooterVictor3.Set(0.0);
ds->PrintfLine(DriverStationLCD::kUser_Line3, "Shooter is Off");
}
}
void LoaderControl(){ //servo control function
if(shooterStick.GetRawButton(3)){
loaderArm.Set(true); //if middle button is pressed, extend servo
}
else{
loaderArm.Set(false); //if middle button isnt pressed, retract servo
}
}
void Climbing(){ //pyramid climbing function, happens automatically
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if( driverStick.GetRawButton(4)){ //if button 6 pressed on both sticks, starts climb
ds->PrintfLine(DriverStationLCD::kUser_Line4, "Arms Up");
SC1.Set(true);
SC2.Set(true);
}
if(driverStick.GetRawButton(5)){
ds->PrintfLine(DriverStationLCD::kUser_Line4, "Arms Down");
SC1.Set(false);
SC2.Set(false);
}
}
/*
void DiskCount(){ //function that counts disk in the hopper and displays it
DriverStationLCD *ds = DriverStationLCD::GetInstance();
int disknum = 0; //variable that counts disks in hopper
if(Disk1.Get()){
disknum=disknum+1; //disk has activated a limit switch, add 1 to # of disks
}
if(Disk2.Get()){
disknum=disknum+1;
}
if(Disk3.Get()){
disknum=disknum+1;
}
ds->PrintfLine(DriverStationLCD::kUser_Line5, "Disk's in Hopper: %d", disknum); //print out the number of disks in hopper
}
*/
};
START_ROBOT_CLASS(RobotDemo); //starts the class running
|
