|
Re: Code not Properly Downloading
Let's see, I'll try and post the code here below. Haven't tried the printf test but have entered debug mode with the robot on blocks and I believe it appeared to follow the code appropriately, but the robot did not respond accordingly. Pretty sure we don't have any uninitialized variables, but I just took this on today. I'll be taking a closer look at the problem today after school and post the results here. Probably will start with using a different computer/windriver reinstall. Thanks for the help.
BTW, this year the students and mentors have been collaborating with code via bitbucket.org. We highly recommend it for tracking changes/reaching mentors at work etc.
Here's the code:
Code:
#include "WPILib.h"
#include "Target.h"
#include "DashboardDataSender.h"
#include "Dashboard.h"
#include "Math.h"
const float ARM_MINIMUM_ROTATION = 7.0f; //7.0f; // don't ever drive the arm lower than this!
const float ARM_MAXIMUM_ROTATION = 650.0f;
const int ARM_PICKUP_PRESET = 135;
const int ARM_HIGH_GOAL_PRESET = 430;
const float ARM_PROPORTIONAL_CONSTANT = 0.0075f;
const int AUTO_ROTATION_START = 345;
const int AUTO_ROTATION_START_FORK = 330;
const int AUTO_ROTATION_SCORE_HIGH = 350;
const int AUTO_ROTATION_LOWER = 50;
const int AUTO_FULL_SPEED_MOVE_ENCODER_TICKS = 2000; //2500 is total ticks start to end
const float AUTO_CLAW_REJECT_SECONDS = 1.5f;
const float AUTO_BACKUP_TIME = 5.0f;
const float AUTO_BACKUP_SECONDS = 4.0f;
const int AUTO_BACKUP_TICKS = 2200;
const int AUTO_TURN_TICKS = 520;
const float CLAW_CAPTURE_SPEED = 0.6f;
const float CLAW_REJECT_SPEED = 0.3f;
const float CLAW_ROTATION_SPEED = 0.3f;
const float AUTO_CLAW_REJECT_SPEED = .2f;
UINT32 Auto_End_Line;
UINT32 Auto_End_Line_Fork;
const float MINIBOT_AUTO_DEPLOY_VOLTAGE = 4.5f;
const float MINIBOT_AUTO_DEPLOY_SECONDS = 110.0f;
//#define MINIMUM_SCORE 0.01
double outputValue;
const UINT32 DETECTSLINE = 1;
const UINT32 LEFT_SENSOR_CHANNEL = 10;
const UINT32 CENTER_SENSOR_CHANNEL = 11;
const UINT32 RIGHT_SENSOR_CHANNEL = 12;
const float AUTO_SPEED_LOW = 0.8f;
const float AUTO_SPEED_FULL = 1.0f;
UINT32 LeftSensorReading;
int XL;
class SamplePIDOutput : public PIDOutput {
public:
SamplePIDOutput(RobotDrive *base) {
m_base = base;
}
void PIDWrite(float output) {
m_base->ArcadeDrive(0.0, output);
outputValue = -output;
}
private:
RobotDrive *m_base;
};
/**
* Demo that demonstrates tracking with the robot.
* In Teleop mode the robot will drive using a single joystick plugged into port 1.
* When the trigger is pressed, the robot will turn to face the circular target.
* This demo uses the AxisCamera class to grab images. It finds the target and computes
* the angle to the target from the center of view in the camera image. It then computes
* a goal correction angle and uses the gyro to turn to that heading. The gyro correction
* is done using the PID class to drive the robot to the desired heading.
* The coefficients for the PID loop will need to be tuned to match the CG and weight of
* your robot as well as the wheel and driving surface types.
*
* As an added bonus, the program also sends robot status information to the LabVIEW sample
* dashboard as well as camera annotation to show the targets that the robot is currently
* seeing.
*/
class RobotDemo : public SimpleRobot {
// RobotDrive *base;
// Joystick *stick; // only joystick
DigitalInput *armUpperLimit; // arm upper limit switch
DigitalInput *armLowerLimit; // arm lower limit switch
AnalogChannel *armUpperLimit_ANALOG;
AnalogChannel *armLowerLimit_ANALOG;
Joystick *rightStick; // joystick 1 (arcade stick or right tank stick)
Joystick *leftStick; // joystick 2 (tank left stick)
Joystick *armStick; // joystick 3 to control arm
DriverStation *ds; // driver station object
Jaguar *LeftDrive;
Jaguar *RightDrive;
Jaguar *ArmRotation;
Jaguar *ArmExtension;
Jaguar *ClawTop;
Jaguar *ClawBottom;
Encoder *LeftDriveEnc;
Encoder *RightDriveEnc;
Encoder *ArmRotationEnc;
Encoder *ArmExtensionEnc;
Compressor *compressor;
Solenoid *ShifterHigh;
Solenoid *ShifterLow;
Solenoid *EndGameIn;
Solenoid *EndGameOut;
Solenoid *DeploymentOut;
Solenoid *DeploymentIn;
DashboardDataSender *dds;
PIDOutput *pidOutput;
Gyro *gyro;
DigitalInput *LeftLineSensor;
DigitalInput *CenterLineSensor;
DigitalInput *RightLineSensor;
DigitalInput *ArmSensor;
float LastExtensionCommand;
AnalogChannel *Minibot_Deploy_Light_Sensor;
//Timer Minibot_Deploy_Timer;
bool Minibot_Deploy_Enabled;
// Arm Position (angle) control
float CurrentArmPosition;
float TargetArmPosition;
float ArmPosition_error;
float ArmPositionCommand;
float ArmPositionConstant;
float CurrentArmStick;
float PreviousArmStick;
float ArmPosMaxCmd;
int Previous_Sensor_Detect;
int AutoSeq;
Timer AutoStateTimer;
int AutoState;
int AutoLeftEncoderStart;
int AutoRightEncoderStart;
int left_drive_count;
int right_drive_count;
enum // Driver Station jumpers to control program operation
{ ARCADE_MODE = 1, // Tank/Arcade jumper is on DS Input 1 (Jumper present is arcade)
ENABLE_AUTONOMOUS = 2, // Autonomous/Teleop jumper is on DS Input 2 (Jumper present is autonomous)
} jumpers;
public:
RobotDemo(void)
{
/* // initialize all variables
base = new RobotDrive(1, 2);
base->SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
base->SetInvertedMotor(RobotDrive::kRearRightMotor, true);
stick = new Joystick(1);
pidOutput = new SamplePIDOutput(base);
base->SetExpiration(0.5);
*/
gyro = new Gyro(1);
dds = new DashboardDataSender();
ds = DriverStation::GetInstance();
rightStick = new Joystick(1); // create the joysticks
leftStick = new Joystick(2);
armStick = new Joystick(3);
armUpperLimit = new DigitalInput(8); // create the limit switch inputs
armLowerLimit = new DigitalInput(9);
armUpperLimit_ANALOG = new AnalogChannel(1);
armLowerLimit_ANALOG = new AnalogChannel(2);
Minibot_Deploy_Light_Sensor = new AnalogChannel(3);
LeftDrive = new Jaguar(1);
RightDrive = new Jaguar(2);
ArmRotation = new Jaguar(3);
ArmExtension = new Jaguar(4);
ClawTop = new Jaguar(5);
ClawBottom = new Jaguar(6);
LeftDriveEnc = new Encoder(2,3);
RightDriveEnc = new Encoder(4,5);
ArmRotationEnc = new Encoder(6,7);
ArmExtensionEnc = new Encoder(13,14);
compressor = new Compressor(1,1); //Pressure Switch, Compressor Relay
compressor->Start();
ShifterHigh = new Solenoid(8,1);
ShifterLow = new Solenoid(8,2);
EndGameIn = new Solenoid(8,4);
EndGameOut = new Solenoid(8,5);
DeploymentOut = new Solenoid(8,7);
DeploymentIn = new Solenoid(8,8);
LeftLineSensor = new DigitalInput(LEFT_SENSOR_CHANNEL);
CenterLineSensor = new DigitalInput(CENTER_SENSOR_CHANNEL);
RightLineSensor = new DigitalInput(RIGHT_SENSOR_CHANNEL);
ArmSensor = new DigitalInput(13);
//Update the motors at least every 100ms.
// myRobot.SetExpiration(0.1);
GetWatchdog().SetExpiration(100); //0.5); //100);
// Arm Rotation Control Variables
CurrentArmPosition = 0.0f;
TargetArmPosition = 0.0f;
ArmPosition_error = 0.0f;
ArmPositionCommand = 0.0f;
ArmPositionConstant = ARM_PROPORTIONAL_CONSTANT; // this controls how fast the arm reacts
CurrentArmStick = 0.0f;
PreviousArmStick = 0.0f;
ArmPosMaxCmd = 1.0f;
ArmRotation->Set(0.0);
}
//ArmExtensionEnc->Get(),
void Dashboard_Update(void)
{
dds->sendIOPortData
(
LeftDriveEnc->Get(),
RightDriveEnc->Get(),
LeftLineSensor->Get(),
CenterLineSensor->Get(),
RightLineSensor->Get(),
ArmRotationEnc->Get(),
TargetArmPosition,
ArmPosition_error,
ArmPositionCommand,
ds->GetAnalogIn(1), //slider
armUpperLimit->Get(),
armLowerLimit->Get(),
armUpperLimit_ANALOG->GetVoltage(),
armLowerLimit_ANALOG->GetVoltage(),
Minibot_Deploy_Light_Sensor->GetVoltage(),
AutoSeq,
AutoState
);
}
void ArmExtensionPref()
{
if(ds->GetDigitalIn(6) == 1)
{
ArmExtension->Set(armStick->GetY());
}
else
{
ArmExtension->Set(armStick->GetTop());
}
}
void ArmRotationPref()
{
if(ds->GetDigitalIn(6) == 1)
{
ArmRotation->Set(armStick->GetX());
}
else
{
ArmRotation->Set(armStick->GetY());
}
}
void UpdateArm()
{
CurrentArmPosition = (float)ArmRotationEnc->Get();
if(TargetArmPosition > 0)
{
ArmPosition_error = TargetArmPosition - CurrentArmPosition;
ArmPositionCommand = ArmPosition_error * ArmPositionConstant; // +.1f;
PreviousArmStick = CurrentArmStick;
CurrentArmStick = armStick->GetX();
// Clamp the command given to the arm motor
if(ArmPositionCommand > ArmPosMaxCmd)
{
ArmPositionCommand = ArmPosMaxCmd;
}
if(ArmPositionCommand < -ArmPosMaxCmd)
{
ArmPositionCommand = -ArmPosMaxCmd;
}
if(fabs(PreviousArmStick) > 0.1f)
{
if(fabs(CurrentArmStick) < 0.1f)
{
// User just stopped using joystick, tell arm controller
// to hold at current position
TargetArmPosition = CurrentArmPosition;
}
else
{
// User is still using the joystick, so set the target
// to zero so the arm control code doesn't do anything
TargetArmPosition = 0;
}
}
}
else
{
ArmPositionCommand = armStick->GetX();
}
// Ok, the command has been computed, now protect the ends of
// the arm's motion (don't let the user drive past either limit)
// If the manual override button is pressed, skip this clamping step
if ((CurrentArmPosition <= ARM_MINIMUM_ROTATION) && (armStick->GetRawButton(7) == 0))
{
// we're at the lower limit, only allow zero or positive commands
ArmPositionCommand = max(0.0f,ArmPositionCommand);
}
else if ((CurrentArmPosition >= ARM_MAXIMUM_ROTATION) && (armStick->GetRawButton(7) == 0))
{
ArmPositionCommand = min(ArmPositionCommand,0.0f);
}
ArmRotation->Set(ArmPositionCommand);
//ArmRotation->Set(0);
}
void SetArm(int Position)
{
TargetArmPosition = Position;
}
void UpdateArmExtension(float command)
{
bool UpperLimitReached = (armUpperLimit_ANALOG->GetVoltage() < 3.0f);
bool LowerLimitReached = (armLowerLimit_ANALOG->GetVoltage() > 3.0f);
if(armStick->GetRawButton(7) == 1)
{
UpperLimitReached = false;
}
float ExtensionCmd = command;
if (UpperLimitReached)
{
if (ExtensionCmd > 0.0f) ExtensionCmd = 0.0f;
}
if (LowerLimitReached)
{
if (ExtensionCmd < 0.0f) ExtensionCmd = 0.0f;
}
ArmExtension->Set(ExtensionCmd);
//ArmExtension->Set(0);
}
void UpdateDriving()
{
LeftDrive->Set(leftStick->GetY());
RightDrive->Set(-rightStick->GetY());
}
void StartEncoders()
{
LeftDriveEnc->Reset();
RightDriveEnc->Reset();
//Enc->Reset();
ArmRotationEnc->Reset();
LeftDriveEnc->Start();
RightDriveEnc->Start();
ArmExtensionEnc->Start();
ArmRotationEnc->Start();
}
void Follow_Line(bool Fork, int Left_Right,float speed_scale) //Left = 0; Right = 1; Straight = 2;
{
if((LeftLineSensor->Get()==1)&&(CenterLineSensor->Get()==1)&&(RightLineSensor->Get()==1))
{
LeftDrive->Set(0.0f);
RightDrive->Set(0.0f);
Auto_End_Line = 1;
}
if((LeftLineSensor->Get() == 1) && (CenterLineSensor->Get() == 1))
{
Auto_End_Line_Fork = 1;
}
else if((LeftLineSensor->Get() == 1) && (RightLineSensor->Get() == 1))
{
Auto_End_Line_Fork = 1;
}
else if((CenterLineSensor->Get() == 1) && (RightLineSensor->Get() == 1))
{
Auto_End_Line_Fork = 1;
}
if(Auto_End_Line == 0)
{
if(LeftLineSensor->Get() == 1)
{
Previous_Sensor_Detect = 1;
//LeftDrive->Set(-AUTO_SPEED_LOW); //<- That is forward
//RightDrive->Set(AUTO_SPEED_FULL);
}
if(RightLineSensor->Get() == 1)
{
Previous_Sensor_Detect = 2;
//LeftDrive->Set(-AUTO_SPEED_FULL);
//RightDrive->Set(AUTO_SPEED_LOW);
}
if(CenterLineSensor->Get() == 1)
{
Previous_Sensor_Detect = 3;
//LeftDrive->Set(-AUTO_SPEED_FULL);
//RightDrive->Set(AUTO_SPEED_FULL);
}
if(Previous_Sensor_Detect == 1)
{
LeftDrive->Set(-AUTO_SPEED_LOW*speed_scale);
RightDrive->Set(AUTO_SPEED_FULL*speed_scale);
}
if(Previous_Sensor_Detect == 2)
{
LeftDrive->Set(-AUTO_SPEED_FULL*speed_scale);
RightDrive->Set(AUTO_SPEED_LOW*speed_scale);
}
if(Previous_Sensor_Detect == 3)
{
LeftDrive->Set(-AUTO_SPEED_FULL*speed_scale);
RightDrive->Set(AUTO_SPEED_FULL*speed_scale);
}
}
else if(Auto_End_Line == 1)
{
LeftDrive->Set(0.0f);
RightDrive->Set(0.0f);
}
}
void Read_Auto_Switch(void)
{
// Check the auto sequence number only in disabled mode! During auto, all
// DS inputs are zeroed.
int seq = 0;
bool Dec1 = !ds->GetDigitalIn(1);
bool Dec2 = !ds->GetDigitalIn(3);
bool Dec4 = !ds->GetDigitalIn(5);
bool Dec8 = !ds->GetDigitalIn(7);
if (Dec1 == true)
{
seq += 1;
}
if (Dec2 == true)
{
seq += 2;
}
if (Dec4 == true)
{
seq += 4;
}
if (Dec8 == true)
{
seq += 8;
}
if (seq != 0) AutoSeq = seq;
}
virtual void Disabled()
{
Read_Auto_Switch();
}
void Auto_Goto_State(int new_state)
{
AutoState = new_state;
AutoStateTimer.Reset();
AutoLeftEncoderStart = LeftDriveEnc->Get();
AutoRightEncoderStart = RightDriveEnc->Get();
}
void AUTO_one()
{
if(AutoState == 0)
{
float speed_scale = 1.0f;
SetArm(AUTO_ROTATION_START);
UpdateArmExtension(-.5);
// watch encoders and slow down when we get close!
// in low gear, about 2500 ticks from start to end
if (left_drive_count > AUTO_FULL_SPEED_MOVE_ENCODER_TICKS)
{
speed_scale = 0.6f;
}
Follow_Line(false,3,speed_scale);
if(Auto_End_Line == 1)
{
Auto_Goto_State(1);
}
}
else if (AutoState == 1)
{
SetArm(AUTO_ROTATION_SCORE_HIGH);
UpdateArmExtension(-.5);
if(abs((int)ArmPosition_error) < 60.0f) //HACK!
{
Auto_Goto_State(2);
}
}
else if (AutoState == 2)
{
// release tube
if (AutoStateTimer.Get() < AUTO_CLAW_REJECT_SECONDS)
{
ClawTop->Set(-AUTO_CLAW_REJECT_SPEED);
ClawBottom->Set(-AUTO_CLAW_REJECT_SPEED);
UpdateArmExtension(0.5);
}
else
{
Auto_Goto_State(3);
ClawTop->Set(0);
ClawBottom->Set(0);
}
}
else if (AutoState == 3)
{
// back up for a few seconds
if (AutoStateTimer.Get() < AUTO_BACKUP_SECONDS) // abs(left_drive_count) > 200)
{
LeftDrive->Set(1.0f); //<- That is forward
RightDrive->Set(-1.0f);
if (AutoStateTimer.Get() > 2.0f)
{
SetArm(AUTO_ROTATION_LOWER); // lower arm after 2 sec
}
}
else
{
LeftDrive->Set(0);
RightDrive->Set(0);
Auto_Goto_State(4);
}
}
else if (AutoState == 4)
{
if (abs(left_drive_count) < AUTO_TURN_TICKS)
{
LeftDrive->Set(-AUTO_SPEED_LOW); //<- That is forward
RightDrive->Set(-AUTO_SPEED_LOW);
}
else
{
LeftDrive->Set(0);
RightDrive->Set(0);
Auto_Goto_State(5);
}
}
}
void AUTO_two_fork(int rightleft) //o = right 1 = left
{
if(AutoState == 0)
{
float speed_scale = 1.0f;
UpdateArmExtension(-.5);
SetArm(AUTO_ROTATION_START_FORK);
// watch encoders and slow down when we get close!
// in low gear, about 2500 ticks from start to end
//if (left_drive_count > AUTO_FULL_SPEED_MOVE_ENCODER_TICKS)
//{
// speed_scale = 0.5f;
//}
Follow_Line(false,3,speed_scale);
Auto_End_Line = 0;
if(left_drive_count > 1640)
{
Auto_Goto_State(1);
LeftDrive->Set(0.0f);
RightDrive->Set(0.0f);
}
}
else if (AutoState == 1)
{
if((CenterLineSensor->Get() == 1) && (RightLineSensor->Get() == 0) && (LeftLineSensor->Get() == 0))
{
Auto_Goto_State(2);
}
else
{
if(rightleft == 0)
{
LeftDrive->Set(-0.35f);
RightDrive->Set(-0.2f);
}
else
{
LeftDrive->Set(0.2f);
RightDrive->Set(.35f);
}
}
}
else if (AutoState == 2)
{
// release tube
if (left_drive_count < 250)
{
Follow_Line(0,0,.5f);
}
else
{
Auto_Goto_State(3);
LeftDrive->Set(0.0f);
RightDrive->Set(0.0f);
}
}
else if (AutoState == 3)
{
if(rightleft == 0)
{
if (left_drive_count > -40)
{
LeftDrive->Set(.3f);
RightDrive->Set(.3f);
}
else
{
Auto_Goto_State(4);
LeftDrive->Set(0.0f);
RightDrive->Set(0.0f);
}
}
else if(rightleft == 1)
{
if (right_drive_count > -40)
{
LeftDrive->Set(-.3f);
RightDrive->Set(-.3f);
}
else
{
Auto_Goto_State(4);
LeftDrive->Set(0.0f);
RightDrive->Set(0.0f);
}
}
}
else if (AutoState == 4)
{
if(left_drive_count < 240)
{
LeftDrive->Set(-.3f);
RightDrive->Set(.3f);
}
else
{
Auto_Goto_State(5);
LeftDrive->Set(0.0f);
RightDrive->Set(0.0f);
}
}
else if(AutoState == 5)
{
if (AutoStateTimer.Get() < AUTO_CLAW_REJECT_SECONDS)
{
ClawTop->Set(-AUTO_CLAW_REJECT_SPEED);
ClawBottom->Set(-AUTO_CLAW_REJECT_SPEED);
UpdateArmExtension(0.5);
}
else
{
Auto_Goto_State(6);
}
}
}
void Auto_Hack(void)
{
LeftDrive->Set(-AUTO_SPEED_FULL);
RightDrive->Set(AUTO_SPEED_FULL);
//Follow_Line(false,3,1.0f);
Dashboard_Update();
//UpdateArm();
}
void Autonomous(void)
{
GetWatchdog().SetEnabled(false);
StartEncoders();
// make sure we're in low gear
//ShifterHigh->Set(true);
//ShifterLow->Set(false);
Previous_Sensor_Detect = 3;
TargetArmPosition = 0; // clear out any arm command
AutoLeftEncoderStart = LeftDriveEnc->Get();
AutoRightEncoderStart = RightDriveEnc->Get();
AutoStateTimer.Reset();
AutoStateTimer.Start();
if (ds->GetDigitalIn(ENABLE_AUTONOMOUS) == 1) // only run the autonomous program if jumper is in place
{
AutoState = 0;
Auto_End_Line = 0;
Auto_End_Line_Fork = 0;
while(IsAutonomous() == true)
{
left_drive_count = LeftDriveEnc->Get() - AutoLeftEncoderStart;
right_drive_count = RightDriveEnc->Get() - AutoRightEncoderStart;
Dashboard_Update();
UpdateArm();
//Auto_Hack();
//AUTO_one();
if(AutoSeq == 1)
{
AUTO_one();
}
else if(AutoSeq == 2)
{
AUTO_two_fork(0);
}
else if(AutoSeq == 3)
{
AUTO_two_fork(1);
}
GetWatchdog().Feed();
}
}
UpdateArmExtension(0.0);
GetWatchdog().SetEnabled(true);
// high gear after auto
//ShifterHigh->Set(false);
//ShifterLow->Set(true);
}
/**
* Main test program in operator control period.
* Aquires images from the camera and determines if they match FRC targets.
*/
void OperatorControl(void) {
/**
* Set up the PID controller with some parameters that should be pretty
* close for most kitbot robots.
*/
// Create and set up a camera instance. first wait for the camera to start
// if the robot was just powered on. This gives the camera time to boot.
StartEncoders();
TargetArmPosition = 0; // clear out any arm command
//Minibot_Deploy_Timer.Reset();
//Minibot_Deploy_Timer.Start();
printf("Getting camera instance\n");
AxisCamera &camera = AxisCamera::GetInstance();
printf("Setting camera parameters\n");
camera.WriteResolution(AxisCamera::kResolution_320x240);
camera.WriteCompression(20);
camera.WriteBrightness(0);
// set sensitivity for the 2010 kit gyro
gyro->SetSensitivity(0.007);
// set MotorSafety expiration
GetWatchdog().SetExpiration(1.0);
// keep track of the previous joystick trigger value
// bool lastTrigger = false;
// loop getting images from the camera and finding targets
printf("Starting operator control loop\n");
Timer timer;
timer.Start();
while (IsOperatorControl())
{
GetWatchdog().Feed();
Auto_End_Line =0;
Dashboard_Update();
LeftSensorReading = LeftLineSensor->Get();
UpdateDriving();
UpdateArm();
// UpdateArmExtension();
static bool ShifterToggle = false;
static int LastTrigger = 0;
int Trigger = rightStick->GetTrigger();
if ((Trigger == 1) && (LastTrigger == 0))
{
ShifterToggle = !ShifterToggle;
}
LastTrigger = Trigger;
if(ShifterToggle)
{
ShifterHigh->Set(true);
ShifterLow->Set(false);
}
else
{
ShifterHigh->Set(false);
ShifterLow->Set(true);
}
if(rightStick->GetRawButton(2) == 1)
{
EndGameOut->Set(false);
// EndGameOut->Set(true);
// EndGameIn->Set(false);
}
else
{
EndGameOut->Set(true);
// EndGameOut->Set(false);
// EndGameIn->Set(true);
}
static bool DeploymentToggle = false;
static int LastButton = 0;
int Button = rightStick->GetRawButton(3);
if ((Button == 1) && (LastButton == 0))
{
DeploymentToggle = !DeploymentToggle;
}
LastButton = Button;
// Auto Deploy for MiniBot
// bool Auto_Deploy = ds->GetAnalogIn(1);
/* if (
(Minibot_Deploy_Timer.Get() > MINIBOT_AUTO_DEPLOY_SECONDS)
&& (Minibot_Deploy_Light_Sensor->GetVoltage() > MINIBOT_AUTO_DEPLOY_VOLTAGE)
&& (Auto_Deploy == true)
)
{
DeploymentToggle = true;
}
*/
if(DeploymentToggle)
{
DeploymentOut->Set(true);
DeploymentIn->Set(false);
}
else
{
DeploymentOut->Set(false);
DeploymentIn->Set(true);
}
UpdateArmExtension(armStick->GetY());
// Preset 1
if(armStick->GetRawButton(12)==1)
{
SetArm(ARM_PICKUP_PRESET); // pickup height
}
// Preset 2
if(armStick->GetRawButton(11)==1)
{
SetArm(ARM_HIGH_GOAL_PRESET); // high goal
}
if (armStick->GetTrigger() == 1)
{
ClawTop->Set(-CLAW_REJECT_SPEED);
ClawBottom->Set(-CLAW_REJECT_SPEED);
if (armUpperLimit_ANALOG->GetVoltage() < 3.0f)
{
ArmExtension->Set(0.0f);
}
else
{
ArmExtension->Set(1.0f);
}
}
else if (armStick->GetRawButton(2) == 1)
{
ClawTop->Set(CLAW_CAPTURE_SPEED);
ClawBottom->Set(CLAW_CAPTURE_SPEED);
}
else if (armStick->GetRawButton(3) == 1)
{
ClawTop->Set(-CLAW_ROTATION_SPEED);
ClawBottom->Set(CLAW_ROTATION_SPEED);
}
else if (armStick->GetRawButton(5) == 1)
{
ClawTop->Set(CLAW_ROTATION_SPEED);
ClawBottom->Set(-CLAW_ROTATION_SPEED);
}
else
{
ClawTop->Set(0.0);
ClawBottom->Set(0.0);
}
// ClawTop->Set(armStick->GetTop());
/* bool trigger;
// if trigger is pulled, the robot will run with standard arcade drive
// otherwise the robot will home towards the target.
if (trigger = stick->GetTrigger()) {
if (trigger != lastTrigger) // check if trigger changed
turnController.Enable();
// if there's a fresh and we're at the previous target heading then
// get a camera image and process it
if (camera.IsFreshImage())
{
timer.Reset();
// get the gyro heading that goes with this image
double gyroAngle = gyro->PIDGet();
// get the camera image
HSLImage *image = camera.GetImage();
// find FRC targets in the image
vector<Target> targets = Target::FindCircularTargets(image);
delete image;
if (targets.size() == 0 || targets[0].m_score < MINIMUM_SCORE)
{
// no targets found. Make sure the first one in the list is 0,0
// since the dashboard program annotates the first target in green
// and the others in magenta. With no qualified targets, they'll all
// be magenta.
Target nullTarget;
nullTarget.m_majorRadius = 0.0;
nullTarget.m_minorRadius = 0.0;
nullTarget.m_score = 0.0;
if (targets.size() == 0)
targets.push_back(nullTarget);
else
targets.insert(targets.begin(), nullTarget);
dds->sendVisionData(0.0, gyro->GetAngle(), 0.0, 0.0, targets);
if (targets.size() == 0)
printf("No target found\n\n");
else
printf("No valid targets found, best score: %f ", targets[0].m_score);
}
else {
// We have some targets.
// set the new PID heading setpoint to the first target in the list
double horizontalAngle = targets[0].GetHorizontalAngle();
double setPoint = gyroAngle + horizontalAngle;
turnController.SetSetpoint(setPoint);
// send dashbaord data for target tracking
dds->sendVisionData(0.0, gyro->GetAngle(), 0.0, targets[0].m_xPos / targets[0].m_xMax, targets);
printf("Target found %f ", targets[0].m_score);
// targets[0].Print();
}
printf("Time: %f\n", 1.0 / timer.Get());
}
} else {
// if the trigger is not pressed, then do Arcade driving with joystick 1
if (trigger != lastTrigger)
turnController.Disable();
base->ArcadeDrive(stick);
}
lastTrigger = trigger;
// send the dashbaord data associated with the I/O ports
dds->sendIOPortData();
*/ }
}
};
START_ROBOT_CLASS(RobotDemo);
|