2014-03-02 03:28:36 +00:00
//Add a button on joystick that activates "auto" to drive to 40 inches away and another to shoot when at 40 inches away (use the little joystick on both drive and shooter stick)
//Sonar in auto: drive till 40in away (dashboard value) and shoot
2014-03-08 22:10:33 +00:00
//Includes{{{
2014-02-22 23:14:01 +00:00
# include "WPILib.h"
# include "SmartDashboard/SmartDashboard.h"
2014-02-15 15:47:49 +00:00
# include <iostream>
# include <math.h>
2014-02-17 01:09:11 +00:00
# include <vector>
2014-02-20 03:25:03 +00:00
# include <sstream>
2014-03-08 22:10:33 +00:00
//}}}
2014-02-15 15:47:49 +00:00
class RobotDemo : public SimpleRobot
{
2014-03-08 22:10:33 +00:00
//Declarations{{{
2014-02-15 15:47:49 +00:00
RobotDrive myRobot ;
2014-03-02 20:32:51 +00:00
float potVal , multiplier , throttle ;
2014-03-07 00:30:25 +00:00
bool collectorExtended , shooting , compressing , allowCompressing ;
2014-03-01 16:54:04 +00:00
float upLimit ;
2014-02-15 15:47:49 +00:00
Joystick Rstick , Lstick ;
Solenoid collectorSole1 , collectorSole2 ;
2014-02-17 01:09:11 +00:00
Compressor compressor ;
2014-02-17 20:04:40 +00:00
Jaguar Left1 , Left2 , Left3 , Right1 , Right2 , Right3 , RightArmMotor1 , RightArmMotor2 , LeftArmMotor1 , LeftArmMotor2 , CollectorMotor1 ;
2014-02-15 15:47:49 +00:00
AnalogChannel armPot ;
2014-02-17 20:04:40 +00:00
AnalogChannel BallSonicLeft , BallSonicRight , WallSonicLeft , WallSonicRight ;
2014-02-22 23:14:01 +00:00
DigitalOutput BallLeft , BallRight , WallLeft , WallRight ;
2014-03-08 22:10:33 +00:00
//}}}
2014-02-15 15:47:49 +00:00
public :
RobotDemo ( ) :
2014-03-08 22:10:33 +00:00
//Initializations{{{
2014-02-15 15:47:49 +00:00
//Joysticks
Rstick ( 1 ) ,
Lstick ( 2 ) ,
//Pot
2014-02-17 01:09:11 +00:00
armPot ( 6 ) ,
2014-02-15 15:47:49 +00:00
//Ultrasonic
BallSonicLeft ( 1 ) ,
BallSonicRight ( 2 ) ,
WallSonicLeft ( 3 ) ,
WallSonicRight ( 4 ) ,
2014-02-22 23:14:01 +00:00
BallLeft ( 1 , 4 ) ,
WallLeft ( 1 , 5 ) ,
BallRight ( 2 , 4 ) ,
WallRight ( 2 , 5 ) ,
2014-02-15 15:47:49 +00:00
//Compressor
2014-02-27 04:02:49 +00:00
compressor ( 2 , 5 , 1 , 1 ) ,
//Solenoids
2014-02-15 15:47:49 +00:00
collectorSole1 ( 1 ) ,
collectorSole2 ( 2 ) ,
2014-03-02 20:32:51 +00:00
//Drive Motors
Left1 ( 1 , 1 ) ,
Left2 ( 1 , 2 ) ,
Left3 ( 1 , 3 ) ,
Right1 ( 2 , 1 ) ,
Right2 ( 2 , 2 ) ,
Right3 ( 2 , 3 ) ,
2014-02-15 15:47:49 +00:00
//Shooter Motors
2014-02-17 20:04:40 +00:00
LeftArmMotor1 ( 1 , 4 ) ,
LeftArmMotor2 ( 1 , 5 ) ,
RightArmMotor1 ( 2 , 4 ) ,
RightArmMotor2 ( 2 , 5 ) ,
2014-02-15 15:47:49 +00:00
//Collector Motor
2014-02-22 23:14:01 +00:00
CollectorMotor1 ( 1 , 6 ) ,
myRobot ( Left1 , Left2 , Right1 , Right2 ) {
GetWatchdog ( ) . SetEnabled ( false ) ;
}
2014-03-08 22:10:33 +00:00
//}}}
//RobotInit{{{
2014-02-15 15:47:49 +00:00
void RobotInit ( ) {
DashboardSetup ( ) ;
multiplier = 1.0f ;
2014-02-27 22:10:36 +00:00
upLimit = 130.0 ;
2014-02-15 15:47:49 +00:00
compressor . Start ( ) ;
2014-02-17 01:09:11 +00:00
shooting = false ;
2014-02-15 15:47:49 +00:00
compressing = true ;
2014-03-07 00:30:25 +00:00
allowCompressing = true ;
2014-02-20 03:25:03 +00:00
throttle = 0 ;
2014-02-15 15:47:49 +00:00
}
2014-03-08 22:10:33 +00:00
//}}}
//DashboardSetup{{{
2014-02-15 15:47:49 +00:00
void DashboardSetup ( ) {
SmartDashboard : : PutNumber ( " Throttle " , throttle ) ;
2014-02-27 19:42:10 +00:00
SmartDashboard : : PutNumber ( " upLimit " , 120.0f ) ;
2014-02-17 01:09:11 +00:00
SmartDashboard : : PutNumber ( " armPot " , potToDegrees ( armPot . GetAverageVoltage ( ) ) ) ;
2014-03-13 18:24:42 -07:00
SmartDashboard : : PutNumber ( " Log Level " , 1 ) ;
2014-03-02 22:39:45 +00:00
//Ultrasonic
2014-02-22 23:14:01 +00:00
SmartDashboard : : PutNumber ( " Wall Left " , voltToDistance ( WallSonicLeft . GetAverageVoltage ( ) , true ) ) ;
SmartDashboard : : PutNumber ( " Wall Right " , voltToDistance ( WallSonicRight . GetAverageVoltage ( ) , true ) ) ;
SmartDashboard : : PutNumber ( " Ball Left " , voltToDistance ( BallSonicLeft . GetAverageVoltage ( ) ) ) ;
SmartDashboard : : PutNumber ( " Ball Right " , voltToDistance ( BallSonicRight . GetAverageVoltage ( ) ) ) ;
2014-03-02 20:32:51 +00:00
//Autonomous values
2014-03-07 01:23:07 +00:00
SmartDashboard : : PutNumber ( " AutoPower " , 0.455f ) ;
SmartDashboard : : PutNumber ( " AutoCorrection " , 0.06f ) ;
2014-03-13 18:24:42 -07:00
SmartDashboard : : PutNumber ( " Inital Drive Timeout " , 2.5f ) ;
2014-03-08 22:10:33 +00:00
SmartDashboard : : PutNumber ( " First Shot Start " , 0.5f ) ;
SmartDashboard : : PutNumber ( " First Shot Stop " , 1.0f ) ;
SmartDashboard : : PutNumber ( " Reverse direction start " , 0.0f ) ;
SmartDashboard : : PutNumber ( " Reverse direction stop " , 2.5f ) ;
SmartDashboard : : PutNumber ( " Second Drive Start " , 1.0f ) ;
SmartDashboard : : PutNumber ( " Second Drive Timeout " , 2.5f ) ;
SmartDashboard : : PutNumber ( " Second Shot Start " , 0.5f ) ;
SmartDashboard : : PutNumber ( " Second Shot Stop " , 1.0f ) ;
2014-03-07 02:10:08 +00:00
SmartDashboard : : PutNumber ( " Autonomous step " , 0.0f ) ;
2014-03-13 18:24:42 -07:00
SmartDashboard : : PutNumber ( " Autonomous sequence " , 0.0f ) ;
2014-03-02 22:39:45 +00:00
//Shooter presets
SmartDashboard : : PutNumber ( " ShortRange " , 0.465f ) ; //Power for the shooter when against the low goal
2014-02-28 20:25:38 +00:00
SmartDashboard : : PutNumber ( " ShooterButtonPower10 " , 0.605f ) ;
SmartDashboard : : PutNumber ( " ShooterButtonPower7 " , 1.0f ) ;
SmartDashboard : : PutNumber ( " ShooterButtonPower8 " , 0.5f ) ;
2014-03-02 22:39:45 +00:00
//Bool switches
2014-03-06 23:45:13 +00:00
SmartDashboard : : PutBoolean ( " OneBallAuto " , false ) ;
SmartDashboard : : PutBoolean ( " Use Ultrasonic " , true ) ;
2014-02-27 04:02:49 +00:00
SmartDashboard : : PutBoolean ( " Daniel Mode " , false ) ;
2014-03-01 16:54:04 +00:00
SmartDashboard : : PutBoolean ( " CollectorState " , false ) ;
2014-03-07 00:30:25 +00:00
SmartDashboard : : PutBoolean ( " Compressor Enabled " , allowCompressing ) ;
2014-03-06 23:44:23 +00:00
SmartDashboard : : PutBoolean ( " Compressor Running " , compressing ) ;
2014-03-07 02:10:08 +00:00
SmartDashboard : : PutBoolean ( " Ignore Pot " , false ) ;
2014-03-06 23:44:23 +00:00
//Battery voltage
2014-02-15 15:47:49 +00:00
}
2014-03-08 22:10:33 +00:00
//}}}
//updateDashboard{{{
2014-02-15 15:47:49 +00:00
void updateDashboard ( ) {
SmartDashboard : : PutNumber ( " Throttle " , throttle ) ;
2014-02-17 01:09:11 +00:00
SmartDashboard : : PutNumber ( " armPot " , potToDegrees ( armPot . GetAverageVoltage ( ) ) ) ;
2014-02-22 23:14:01 +00:00
SmartDashboard : : PutNumber ( " Wall Left " , voltToDistance ( WallSonicLeft . GetAverageVoltage ( ) , true ) ) ;
SmartDashboard : : PutNumber ( " Wall Right " , voltToDistance ( WallSonicRight . GetAverageVoltage ( ) , true ) ) ;
SmartDashboard : : PutNumber ( " Ball Left " , voltToDistance ( BallSonicLeft . GetAverageVoltage ( ) ) ) ;
SmartDashboard : : PutNumber ( " Ball Right " , voltToDistance ( BallSonicRight . GetAverageVoltage ( ) ) ) ;
2014-02-28 20:25:38 +00:00
SmartDashboard : : PutNumber ( " upLimit " , upLimit ) ;
2014-03-06 23:44:23 +00:00
SmartDashboard : : PutBoolean ( " Compressor Running " , compressing ) ;
2014-03-07 00:30:25 +00:00
allowCompressing = SmartDashboard : : GetBoolean ( " Compressor Enabled " ) ;
2014-02-27 04:02:49 +00:00
if ( upLimit > 167 ) {
2014-02-17 01:09:11 +00:00
upLimit = 167 ;
}
2014-02-15 15:47:49 +00:00
}
2014-03-08 22:10:33 +00:00
//}}}
//shootRobot{{{
void shootRobot ( float power = 0 ) {
setMotorValue ( 4 , 1 , cvt ( power ) ) ;
setMotorValue ( 5 , 1 , cvt ( power ) ) ;
setMotorValue ( 4 , 2 , cvt ( - power ) ) ;
setMotorValue ( 5 , 2 , cvt ( - power ) ) ;
}
//}}}
//driveRobot{{{
void driveRobot ( float x , float y ) {
if ( y > 1.0f ) {
y = 1.0f ;
} else if ( y ! = 0.0f & & y < - 1.0f ) {
y = - 1.0f ;
}
int leftPower = ( ( y + x ) / 2 + 1 ) * 127 + 1 ;
int rightPower = ( ( y - x ) / 2 + 1 ) * 127 + 1 ;
setMotorValue ( 1 , 1 , leftPower ) ;
setMotorValue ( 2 , 1 , leftPower ) ;
setMotorValue ( 3 , 1 , leftPower ) ;
setMotorValue ( 1 , 2 , rightPower ) ;
setMotorValue ( 2 , 2 , rightPower ) ;
setMotorValue ( 3 , 2 , rightPower ) ;
}
//}}}
//voltToDistance{{{
2014-02-22 23:14:01 +00:00
float voltToDistance ( float a , bool wall = false ) {
2014-02-27 04:02:49 +00:00
if ( wall ) {
2014-02-22 23:14:01 +00:00
return ( a / 0.00488f ) / 2.54f ;
2014-02-27 04:02:49 +00:00
} else {
2014-02-22 23:14:01 +00:00
return ( a / 0.000976562f ) / 25.4f ;
2014-02-20 03:25:03 +00:00
}
}
2014-03-08 22:10:33 +00:00
//}}}
//potToDegrees{{{
2014-02-20 03:25:03 +00:00
float potToDegrees ( float a ) {
2014-02-17 01:09:11 +00:00
float max = - .0003948 ;
float min = 5.0245547 ;
2014-02-17 20:04:40 +00:00
float b = a - max ;
min = min - max ; // ~5.0027
max = max - max ; //=0
return 300 - ( ( b + max ) * ( 300 / min ) ) ;
2014-02-17 01:09:11 +00:00
}
2014-03-08 22:10:33 +00:00
//}}}
//cvt{{{
2014-02-15 15:47:49 +00:00
int cvt ( float input ) {
2014-02-17 20:04:40 +00:00
return input * 127.0f + 128 ;
2014-02-15 15:47:49 +00:00
}
2014-03-08 22:10:33 +00:00
//}}}
//setMotorValue{{{
2014-02-17 20:04:40 +00:00
void setMotorValue ( int motor , int subwayStation = 1 , int value = 127 ) {
2014-02-27 04:02:49 +00:00
if ( subwayStation = = 1 ) {
2014-03-08 22:10:33 +00:00
//subwayStation1{{{
2014-02-27 04:02:49 +00:00
switch ( motor ) {
case 1 :
Left1 . SetRaw ( value ) ;
break ;
case 2 :
Left2 . SetRaw ( value ) ;
break ;
case 3 :
Left3 . SetRaw ( value ) ;
break ;
case 4 :
LeftArmMotor1 . SetRaw ( value ) ;
break ;
case 5 :
LeftArmMotor2 . SetRaw ( value ) ;
break ;
case 6 :
CollectorMotor1 . SetRaw ( value ) ;
break ;
case 7 :
break ;
case 8 :
break ;
case 9 :
break ;
case 10 :
break ;
2014-02-15 15:47:49 +00:00
}
2014-03-08 22:10:33 +00:00
//}}}
2014-02-27 04:02:49 +00:00
} else if ( subwayStation = = 2 ) {
2014-03-08 22:10:33 +00:00
//subwayStation2{{{
2014-02-27 04:02:49 +00:00
switch ( motor ) {
//Shooter motors
case 1 :
Right1 . SetRaw ( value ) ;
break ;
case 2 :
Right2 . SetRaw ( value ) ;
break ;
case 3 :
Right3 . SetRaw ( value ) ;
break ;
case 4 :
RightArmMotor1 . SetRaw ( value ) ;
break ;
case 5 :
RightArmMotor2 . SetRaw ( value ) ;
break ;
case 6 :
break ;
case 7 :
break ;
case 8 :
break ;
case 9 :
break ;
case 10 :
break ;
2014-02-15 15:47:49 +00:00
}
2014-03-08 22:10:33 +00:00
//}}}
2014-02-15 15:47:49 +00:00
}
}
2014-03-08 22:10:33 +00:00
//}}}
//Autonomous{{{
2014-02-15 15:47:49 +00:00
void Autonomous ( ) {
2014-03-08 22:10:33 +00:00
//Initializations{{{
2014-02-15 15:47:49 +00:00
myRobot . SetSafetyEnabled ( false ) ;
2014-02-22 23:14:01 +00:00
int i = 0 ;
2014-03-06 23:45:13 +00:00
int c = 0 ;
2014-03-07 00:54:28 +00:00
float power = SmartDashboard : : GetNumber ( " AutoPower " ) ;
float correction = SmartDashboard : : GetNumber ( " AutoCorrection " ) ;
2014-03-07 01:23:07 +00:00
int currentStep = 0 ;
2014-02-26 20:50:12 +00:00
compressing = false ;
collectorSole1 . Set ( false ) ;
collectorSole2 . Set ( true ) ;
2014-02-22 23:14:01 +00:00
WallLeft . Set ( 1 ) ;
2014-02-26 20:50:12 +00:00
BallLeft . Set ( 0 ) ;
WallRight . Set ( 1 ) ;
2014-02-22 23:14:01 +00:00
BallRight . Set ( 0 ) ;
2014-03-01 16:54:04 +00:00
SmartDashboard : : PutBoolean ( " CollectorState " , true ) ;
2014-03-08 22:10:33 +00:00
//}}}
2014-02-27 04:02:49 +00:00
while ( IsEnabled ( ) & & IsAutonomous ( ) ) {
2014-03-13 14:50:58 -07:00
if ( SmartDashboard : : GetNumber ( " Autonomous sequence " ) = = 0 ) {
2014-03-08 22:10:33 +00:00
//Autonomous0{{{
//Drive{{{
if ( currentStep = = 0 ) {
2014-03-13 18:24:42 -07:00
if ( voltToDistance ( WallSonicLeft . GetAverageVoltage ( ) , true ) > = 40.0f ) {
2014-03-13 14:50:58 -07:00
driveRobot ( - 1.0f , correction ) ;
2014-03-08 22:10:33 +00:00
} else {
driveRobot ( 0.0f , 0.0f ) ;
}
if ( c = = SmartDashboard : : GetNumber ( " Inital Drive Timeout " ) * 200 ) {
driveRobot ( 0.0f , 0.0f ) ;
currentStep + + ;
c = 0 ;
}
2014-03-02 22:39:45 +00:00
}
2014-03-08 22:10:33 +00:00
//}}}
//Shoot{{{
if ( currentStep = = 1 & & c > SmartDashboard : : GetNumber ( " First Shot Start " ) * 200 ) {
if ( SmartDashboard : : GetBoolean ( " Ignore Pot " ) | | upLimit > = potToDegrees ( armPot . GetAverageVoltage ( ) ) ) {
shootRobot ( power ) ;
} else {
shootRobot ( 0.0f ) ;
}
2014-03-10 20:05:16 -04:00
setMotorValue ( 6 , 1 , 1 ) ;
2014-03-08 22:10:33 +00:00
if ( c = = ( SmartDashboard : : GetNumber ( " First Shot Stop " ) ) * 200 ) {
shootRobot ( 0.0f ) ;
2014-03-10 20:05:16 -04:00
setMotorValue ( 6 , 1 , 0 ) ;
2014-03-08 22:10:33 +00:00
currentStep + + ;
c = 0 ;
}
2014-02-28 16:02:53 +00:00
}
2014-03-08 22:10:33 +00:00
//}}}
//Lower Shooter{{{
if ( currentStep = = 2 & & c > SmartDashboard : : GetNumber ( " Reverse direction start " ) * 200 ) {
2014-03-13 18:24:42 -07:00
driveRobot ( - 1.0f , correction ) ;
2014-03-08 22:10:33 +00:00
if ( 40.0f < = potToDegrees ( armPot . GetAverageVoltage ( ) ) ) {
2014-03-10 20:05:16 -04:00
shootRobot ( - 0.3f ) ;
} else {
shootRobot ( 0.0f ) ;
}
if ( c = = ( 3.0f ) * 200 ) {
shootRobot ( 0.0f ) ;
2014-03-08 22:10:33 +00:00
currentStep + + ;
c = 0 ;
}
2014-03-07 01:23:07 +00:00
}
2014-03-08 22:10:33 +00:00
//}}}
//}}}
2014-03-13 14:50:58 -07:00
} else if ( SmartDashboard : : GetNumber ( " Autonomous sequence " ) = = 1 ) {
2014-03-08 22:10:33 +00:00
//Autonomous1{{{
//Drive{{{
if ( currentStep = = 0 ) {
2014-03-13 18:24:42 -07:00
if ( voltToDistance ( WallSonicLeft . GetAverageVoltage ( ) , true ) > = 40.0f ) {
2014-03-13 14:50:58 -07:00
driveRobot ( - 1.0f , correction ) ;
2014-03-08 22:10:33 +00:00
} else {
driveRobot ( 0.0f , 0.0f ) ;
}
if ( c = = SmartDashboard : : GetNumber ( " Inital Drive Timeout " ) * 200 ) {
driveRobot ( 0.0f , 0.0f ) ;
currentStep + + ;
c = 0 ;
}
}
//}}}
//Shoot{{{
if ( currentStep = = 1 & & c > SmartDashboard : : GetNumber ( " First Shot Start " ) * 200 ) {
if ( SmartDashboard : : GetBoolean ( " Ignore Pot " ) | | upLimit > = potToDegrees ( armPot . GetAverageVoltage ( ) ) ) {
shootRobot ( power ) ;
} else {
shootRobot ( 0.0f ) ;
}
2014-03-10 20:05:16 -04:00
setMotorValue ( 6 , 1 , 1 ) ;
2014-03-08 22:10:33 +00:00
if ( c = = ( SmartDashboard : : GetNumber ( " First Shot Stop " ) ) * 200 ) {
shootRobot ( 0.0f ) ;
2014-03-10 20:05:16 -04:00
setMotorValue ( 6 , 1 , 0 ) ;
2014-03-08 22:10:33 +00:00
currentStep + + ;
c = 0 ;
}
}
//}}}
//Drive Backwards{{{
if ( currentStep = = 2 & & c > SmartDashboard : : GetNumber ( " Reverse direction start " ) * 200 ) {
2014-03-13 14:50:58 -07:00
driveRobot ( 1.0f , correction ) ;
2014-03-10 20:05:16 -04:00
if ( 40.0f < = potToDegrees ( armPot . GetAverageVoltage ( ) ) ) {
shootRobot ( - 0.2f ) ;
} else {
shootRobot ( 0.0f ) ;
}
2014-03-08 22:10:33 +00:00
if ( c = = ( SmartDashboard : : GetNumber ( " Reverse direction stop " ) ) * 200 ) {
driveRobot ( 0.0f , 0.0f ) ;
2014-03-10 20:05:16 -04:00
shootRobot ( 0.0f ) ;
2014-03-08 22:10:33 +00:00
currentStep + + ;
c = 0 ;
}
}
//}}}
//Drive{{{
if ( currentStep = = 3 & & c > SmartDashboard : : GetNumber ( " Second Drive Start " ) * 200 ) {
2014-03-13 18:24:42 -07:00
if ( voltToDistance ( WallSonicLeft . GetAverageVoltage ( ) , true ) > = 40.0f ) {
2014-03-13 14:50:58 -07:00
driveRobot ( - 1.0f , correction ) ;
2014-03-08 22:10:33 +00:00
} else {
driveRobot ( 0.0f , 0.0f ) ;
}
if ( c = = ( SmartDashboard : : GetNumber ( " Inital Drive Timeout " ) ) * 200 ) {
driveRobot ( 0.0f , 0.0f ) ;
currentStep + + ;
c = 0 ;
}
}
//}}}
//Shoot{{{
if ( currentStep = = 4 & & c > ( SmartDashboard : : GetNumber ( " Second Shot Start " ) ) * 200 ) {
if ( SmartDashboard : : GetBoolean ( " Ignore Pot " ) | | upLimit > = potToDegrees ( armPot . GetAverageVoltage ( ) ) ) {
shootRobot ( power ) ;
} else {
shootRobot ( 0.0f ) ;
}
2014-03-10 20:05:16 -04:00
setMotorValue ( 6 , 1 , 1 ) ;
2014-03-08 22:10:33 +00:00
if ( c = = ( SmartDashboard : : GetNumber ( " First Shot Stop " ) ) * 200 ) {
shootRobot ( 0.0f ) ;
2014-03-10 20:05:16 -04:00
setMotorValue ( 6 , 1 , 0 ) ;
2014-03-08 22:10:33 +00:00
currentStep + + ;
c = 0 ;
}
}
//}}}
//}}}
2014-03-13 14:50:58 -07:00
} else if ( SmartDashboard : : GetNumber ( " Autonomous sequence " ) = = 2 ) {
2014-03-08 22:10:33 +00:00
//Autonomous2{{{
//Drive{{{
2014-03-13 18:24:42 -07:00
if ( currentStep = = 0 ) {
if ( voltToDistance ( WallSonicLeft . GetAverageVoltage ( ) , true ) > = 40.0f ) {
2014-03-13 14:50:58 -07:00
driveRobot ( - 1.0f , correction ) ;
2014-03-08 22:10:33 +00:00
} else {
driveRobot ( 0.0f , 0.0f ) ;
}
if ( c = = SmartDashboard : : GetNumber ( " Inital Drive Timeout " ) * 200 ) {
driveRobot ( 0.0f , 0.0f ) ;
currentStep + + ;
c = 0 ;
}
}
//}}}
//Release Ball{{{
if ( currentStep = = 1 ) {
setMotorValue ( 6 , 1 , 102 ) ;
if ( c = = 50 ) {
currentStep + + ;
c = 0 ;
}
}
//}}}
//Shoot{{{
if ( currentStep = = 2 & & c > SmartDashboard : : GetNumber ( " First Shot Start " ) * 200 ) {
if ( SmartDashboard : : GetBoolean ( " Ignore Pot " ) | | upLimit > = potToDegrees ( armPot . GetAverageVoltage ( ) ) ) {
shootRobot ( power ) ;
} else {
shootRobot ( 0.0f ) ;
}
2014-03-10 20:05:16 -04:00
setMotorValue ( 6 , 1 , 1 ) ;
2014-03-08 22:10:33 +00:00
if ( c = = SmartDashboard : : GetNumber ( " First Shot Stop " ) * 200 ) {
shootRobot ( 0.0f ) ;
2014-03-10 20:05:16 -04:00
setMotorValue ( 6 , 1 , 0 ) ;
2014-03-08 22:10:33 +00:00
currentStep + + ;
c = 0 ;
}
2014-02-27 04:02:49 +00:00
}
2014-03-08 22:10:33 +00:00
//}}}
//Lower Shooter{{{
if ( currentStep = = 3 ) {
2014-03-10 20:05:16 -04:00
if ( 40.0f < = potToDegrees ( armPot . GetAverageVoltage ( ) ) ) {
shootRobot ( - 0.3f ) ;
} else {
shootRobot ( 0.0f ) ;
}
2014-03-08 22:10:33 +00:00
if ( c = = 1 * 200 ) {
shootRobot ( 0.0f ) ;
currentStep + + ;
c = 0 ;
}
}
//}}}
//Collect Ball{{{
if ( currentStep = = 4 & & c > SmartDashboard : : GetNumber ( " Second Shot Start " ) ) {
setMotorValue ( 6 , 1 , 1 ) ;
if ( c = = SmartDashboard : : GetNumber ( " Second Shot Stop " ) * 200 ) {
2014-03-10 20:05:16 -04:00
setMotorValue ( 6 , 1 , 0 ) ;
currentStep + + ;
c = 0 ;
2014-03-08 22:10:33 +00:00
}
}
//}}}
//Shoot{{{
if ( currentStep = = 5 & & c > ( SmartDashboard : : GetNumber ( " Second Shot Start " ) ) * 200 ) {
if ( SmartDashboard : : GetBoolean ( " Ignore Pot " ) | | upLimit > = potToDegrees ( armPot . GetAverageVoltage ( ) ) ) {
shootRobot ( power ) ;
} else {
shootRobot ( 0.0f ) ;
}
2014-03-10 20:05:16 -04:00
setMotorValue ( 6 , 1 , 1 ) ;
2014-03-08 22:10:33 +00:00
if ( c = = ( SmartDashboard : : GetNumber ( " First Shot Stop " ) ) * 200 ) {
2014-03-10 20:05:16 -04:00
setMotorValue ( 6 , 1 , 0 ) ;
2014-03-08 22:10:33 +00:00
shootRobot ( 0.0f ) ;
currentStep + + ;
c = 0 ;
}
}
//}}}
//}}}
2014-02-27 22:10:36 +00:00
}
2014-03-08 22:10:33 +00:00
SmartDashboard : : PutNumber ( " Autonomous step " , currentStep ) ;
2014-02-17 01:09:11 +00:00
updateDashboard ( ) ;
2014-03-08 22:10:33 +00:00
//Compressor{{{
2014-02-27 04:02:49 +00:00
if ( i % 100 = = 0 & & compressing & & compressor . GetPressureSwitchValue ( ) = = 1 ) {
2014-02-17 01:09:11 +00:00
compressor . Stop ( ) ;
compressing = false ;
}
2014-02-27 04:02:49 +00:00
if ( i % 100 = = 0 & & ! compressing & & compressor . GetPressureSwitchValue ( ) = = 0 ) {
2014-02-17 01:09:11 +00:00
compressor . Start ( ) ;
2014-02-17 20:04:40 +00:00
compressing = true ;
2014-02-17 01:09:11 +00:00
}
2014-03-08 22:10:33 +00:00
//}}}
2014-02-20 03:25:03 +00:00
i + + ;
2014-03-06 23:44:23 +00:00
c + + ;
2014-03-08 22:10:33 +00:00
Wait ( 0.005f ) ;
2014-02-22 23:14:01 +00:00
}
2014-02-17 20:04:40 +00:00
compressing = false ;
2014-02-15 15:47:49 +00:00
compressor . Stop ( ) ;
}
2014-03-08 22:10:33 +00:00
//}}}
//Teleop{{{
2014-02-15 15:47:49 +00:00
void OperatorControl ( ) {
2014-03-08 22:10:33 +00:00
//Initializations{{{
2014-02-15 15:47:49 +00:00
myRobot . SetSafetyEnabled ( false ) ;
2014-02-17 20:04:40 +00:00
int i = 0 ;
2014-02-15 15:47:49 +00:00
collectorSole1 . Set ( true ) ;
collectorSole2 . Set ( false ) ;
2014-02-17 20:04:40 +00:00
compressing = false ;
2014-03-01 16:54:04 +00:00
SmartDashboard : : PutBoolean ( " CollectorState " , false ) ;
2014-03-08 22:10:33 +00:00
//}}}
2014-02-27 04:02:49 +00:00
while ( IsEnabled ( ) & & IsOperatorControl ( ) ) {
2014-03-08 22:10:33 +00:00
//Joystick{{{
2014-03-13 17:11:00 -07:00
//Shooter{{{
2014-02-28 20:25:38 +00:00
if ( Lstick . GetRawButton ( 9 ) = = 1 ) {
throttle = ( - Lstick . GetRawAxis ( 4 ) + 1 ) / 2 ;
} else if ( Lstick . GetRawButton ( 10 ) ) {
throttle = SmartDashboard : : GetNumber ( " ShooterButtonPower10 " ) ;
} else if ( Lstick . GetRawButton ( 7 ) ) {
throttle = SmartDashboard : : GetNumber ( " ShooterButtonPower7 " ) ;
} else if ( Lstick . GetRawButton ( 8 ) ) {
throttle = SmartDashboard : : GetNumber ( " ShooterButtonPower8 " ) ;
}
2014-03-13 17:11:00 -07:00
//}}}
//Shooter upLimit{{{
2014-02-27 19:42:10 +00:00
if ( Lstick . GetRawButton ( 3 ) ) {
2014-02-27 22:10:36 +00:00
upLimit = 100.0f ;
2014-02-27 19:42:10 +00:00
}
if ( Lstick . GetRawButton ( 4 ) ) {
2014-02-27 22:10:36 +00:00
upLimit = 120.0f ;
2014-02-27 19:42:10 +00:00
}
if ( Lstick . GetRawButton ( 5 ) ) {
2014-02-27 22:10:36 +00:00
upLimit = 90.0f ;
2014-02-27 19:42:10 +00:00
}
if ( Lstick . GetRawButton ( 6 ) ) {
2014-02-27 22:10:36 +00:00
upLimit = 130.0f ;
2014-02-27 19:42:10 +00:00
}
2014-03-13 17:11:00 -07:00
//}}}
2014-03-08 22:10:33 +00:00
if ( Lstick . GetRawButton ( 1 ) = = 1 ) {
//Shoot{{{
2014-03-06 23:44:23 +00:00
shooting = true ;
shootRobot ( throttle ) ;
setMotorValue ( 6 , 1 , 1 ) ;
2014-03-07 02:10:08 +00:00
if ( collectorExtended = = false ) {
shooting = false ;
}
2014-03-08 22:10:33 +00:00
if ( collectorExtended = = true & & ( SmartDashboard : : GetBoolean ( " Ignore Pot " ) | | upLimit > = potToDegrees ( armPot . GetAverageVoltage ( ) ) ) ) {
2014-03-07 02:10:08 +00:00
shooting = true ;
shootRobot ( throttle ) ;
setMotorValue ( 6 , 1 , 1 ) ;
2014-03-13 18:19:42 -07:00
} else {
shooting = true ;
shootRobot ( 0.0f ) ;
setMotorValue ( 6 , 1 , 0 ) ;
2014-03-07 02:10:08 +00:00
}
2014-03-08 22:10:33 +00:00
//}}}
2014-02-27 04:02:49 +00:00
} else if ( Lstick . GetRawButton ( 2 ) = = 1 ) {
2014-03-08 22:10:33 +00:00
//Lower Shooter{{{
2014-02-17 01:09:11 +00:00
shooting = false ;
2014-03-06 23:44:23 +00:00
shootRobot ( - 0.1f ) ;
2014-03-07 02:10:08 +00:00
if ( collectorExtended = = true ) {
shootRobot ( - 0.1f ) ;
}
2014-03-08 22:10:33 +00:00
//}}}
2014-02-27 04:02:49 +00:00
} else {
2014-03-08 22:10:33 +00:00
//Stop Shooting{{{
2014-02-17 20:04:40 +00:00
shooting = false ;
2014-02-28 20:25:38 +00:00
shootRobot ( 0 ) ;
2014-03-08 22:10:33 +00:00
//}}}
2014-02-15 15:47:49 +00:00
}
2014-02-27 04:02:49 +00:00
if ( Rstick . GetRawButton ( 9 ) = = 1 ) {
2014-03-08 22:10:33 +00:00
//Extend Collector{{{
2014-03-01 16:54:04 +00:00
SmartDashboard : : PutBoolean ( " CollectorState " , true ) ;
2014-02-15 15:47:49 +00:00
collectorExtended = true ;
collectorSole1 . Set ( false ) ;
collectorSole2 . Set ( true ) ;
2014-03-08 22:10:33 +00:00
//}}}
2014-02-27 04:02:49 +00:00
} else if ( Rstick . GetRawButton ( 10 ) = = 1 ) {
2014-03-08 22:10:33 +00:00
//Retract Collector{{{
2014-03-01 16:54:04 +00:00
SmartDashboard : : PutBoolean ( " CollectorState " , false ) ;
2014-02-15 15:47:49 +00:00
collectorExtended = false ;
collectorSole1 . Set ( true ) ;
collectorSole2 . Set ( false ) ;
2014-03-08 22:10:33 +00:00
//}}}
2014-02-15 15:47:49 +00:00
}
2014-03-08 22:10:33 +00:00
//Collector Motor{{{
2014-02-27 04:02:49 +00:00
if ( Lstick . GetRawButton ( 11 ) = = 1 ) {
2014-02-17 20:04:40 +00:00
setMotorValue ( 6 , 1 , 1 ) ;
2014-02-27 04:02:49 +00:00
} else if ( Lstick . GetRawButton ( 12 ) = = 1 ) {
2014-02-17 20:04:40 +00:00
setMotorValue ( 6 , 1 , 255 ) ;
2014-02-27 04:02:49 +00:00
} else if ( ! shooting ) {
2014-02-20 03:25:03 +00:00
setMotorValue ( 6 , 1 , 0 ) ;
2014-02-15 15:47:49 +00:00
}
2014-03-08 22:10:33 +00:00
//}}}
//}}}
//Driving{{{
if ( Rstick . GetRawButton ( 1 ) ) {
if ( voltToDistance ( WallSonicLeft . GetAverageVoltage ( ) , true ) > = 40.0f ) {
driveRobot ( 1.0f , SmartDashboard : : GetNumber ( " AutoCorrection " ) ) ;
} else {
driveRobot ( 0.0f , 0.0f ) ;
}
} else {
if ( SmartDashboard : : GetBoolean ( " Daniel Mode " ) ) {
driveRobot ( - Rstick . GetY ( ) , Rstick . GetZ ( ) + Rstick . GetX ( ) ) ;
} else {
driveRobot ( Rstick . GetY ( ) , Rstick . GetZ ( ) + Rstick . GetX ( ) ) ;
}
}
//}}}
//Compressor{{{
if ( SmartDashboard : : GetBoolean ( " Compressor Enabled " ) ) {
if ( i % 100 = = 0 & & compressing & & compressor . GetPressureSwitchValue ( ) = = 1 ) {
compressor . Stop ( ) ;
compressing = false ;
}
if ( i % 100 = = 0 & & ! compressing & & compressor . GetPressureSwitchValue ( ) = = 0 ) {
compressor . Start ( ) ;
compressing = true ;
}
}
//}}}
updateDashboard ( ) ;
2014-02-15 15:47:49 +00:00
i + + ;
Wait ( 0.005f ) ;
}
2014-03-08 22:10:33 +00:00
compressing = false ;
compressor . Stop ( ) ;
2014-02-15 15:47:49 +00:00
}
2014-03-08 22:10:33 +00:00
//}}}
2014-03-13 17:11:00 -07:00
//Test{{{
void Test ( ) {
}
//}}}
2014-02-15 15:47:49 +00:00
} ;
START_ROBOT_CLASS ( RobotDemo ) ;