Code:
#include "WPILib.h"
#include "Vision/AxisCameraParams.h"
#include "math.h"
#include "CustomMath/CustomMath.h"
#include "C:/WindRiver/workspace/2011_IceRobotics/Xbox360/Xbox360.h"
class RobotDemo : public SimpleRobot
{
RobotDrive myRobot; // robot drive system
//Joystick leftstick; // left joystick
//Joystick rightstick; // right joystick
Joystick armstick;
HSLImage image; // camera
DigitalInput *left; // digital inputs for line tracking sensors
DigitalInput *middle;
DigitalInput *right;
Victor vicShoulder;
Victor vicElbow;
Victor vicWrist;
Victor vicClaw;
Xbox360 xbox;
DigitalInput *sfsensor; //front sensor
DigitalInput *sbsensor; //back sensor
DigitalInput *efsensor; //front sensor
DigitalInput *ebsensor; //back sensor
DigitalInput *wfsensor; //front sensor
DigitalInput *wbsensor; //back sensor
Gyro gyro;
//ADXL345_I2C acc;
//Encoder En;
public:
RobotDemo(void):
myRobot(1, 2, 3, 4), // these must be initialized in the same order
//leftstick(1), // as they are declared above.
//rightstick(2), // as they are declared above.
armstick(3),
vicShoulder(6),
vicElbow(7),
vicWrist(8),
vicClaw(9),
xbox(1),
gyro(1,1)
//acc(1),
//En()
{
myRobot.SetExpiration(0.1);
left = new DigitalInput(2);
middle = new DigitalInput(1);
right = new DigitalInput(3);
sfsensor = new DigitalInput(6,1);
sbsensor = new DigitalInput(6,2);
efsensor = new DigitalInput(6,3);
ebsensor = new DigitalInput(6,4);
wfsensor = new DigitalInput(6,5);
wbsensor = new DigitalInput(6,5);
}
void Autonomous(void)
{
myRobot.SetSafetyEnabled(false);
//gyro.Reset();
//gyro.GetAngle(); // current heading (0 = target)
AxisCamera &robocam = AxisCamera::GetInstance();
robocam.WriteResolution((AxisCamera::Resolution_t)3);
robocam.WriteBrightness(0);
while(IsAutonomous()){
// read digital inputs
unsigned short leftValue = left->Get() ? 4 : 0; // read the line tracking sensors
unsigned short middleValue = middle->Get() ? 2 : 0;
unsigned short rightValue = right->Get() ? 1 : 0;
//begin Line tracking
switch(leftValue + middleValue + rightValue){
//void MecanumDrive_Cartesian(float x, float rotation, float y, float gyroAngle = 0.0);
case(0x0): // normal operation
case(0x5):
case(0x7):{
//This and 0x0, 0x5, are all the messages recieved if there is no line found
break;
}
case(0x1): // straif right
case(0x3):{
//0x1, and 0x3 for when the robot sees the line to the right
//Checks to see what the middle value is, and runs until the middle sensor returns true or sees the line
while (middleValue != true){
myRobot.ArcadeDrive(0.0, 0.50);
}
break;
}
case(0x2):{
//This is the case for the middle
while (middleValue == true){
myRobot.ArcadeDrive(0.5, 0.0);
}
break;
}
case(0x4): // straif left
case(0x6):{
//This case statment if for when it sees the line on the left sensor
while (middleValue != true){
myRobot.ArcadeDrive(0.0, -0.5);
}
break;
}
default:{
break;
}
}
if (leftValue + middleValue + rightValue == 0x0 || 0x5 || 0x7){
//The line has ended and this code is for the arm
}
}
Wait(3.0);
}
void OperatorControl(void)
{
myRobot.SetSafetyEnabled(true);
AxisCamera &robocam = AxisCamera::GetInstance();
robocam.WriteResolution((AxisCamera::Resolution_t)3);
robocam.WriteBrightness(0);
gyro.Reset();
Wait(3.0);
while (IsOperatorControl())
{
// read digital inputs
unsigned short leftValue = left->Get() ? 4 : 0; // read the line tracking sensors
unsigned short middleValue = middle->Get() ? 2 : 0;
unsigned short rightValue = right->Get() ? 1 : 0;
//Look for line sensor inputs only when selected
if (xbox.GetRightBumper()){
switch(leftValue + middleValue + rightValue){
case(0x0): // normal operation
case(0x5):
case(0x7):{
//printf("no line found \n");
//void MecanumDrive_Cartesian(float x, float rotation, float y, float gyroAngle = 0.0);
if (!xbox.GetLeftBumper()){
myRobot.MecanumDrive_Cartesian(-xbox.GetRightX(), -xbox.GetLeftX(), -xbox.GetLeftY(), 0); //Mecanum drive at full speed
}
else{
myRobot.MecanumDrive_Cartesian(-xbox.GetLeftX(), -xbox.GetRightX(), -xbox.GetLeftY(), 0);//Mecanumdrive at half speed
}
break;
}
case(0x1): // straif right
case(0x3):{
// printf("right leftstickX = %f, abs = %i\n",leftstick.GetX(), abs((int)leftstick.GetX()));
float leftvalue = (fabs(xbox.GetLeftX())/-2);
//printf("right leftstickX = %f\n", leftvalue);
myRobot.MecanumDrive_Cartesian(leftvalue, 0, -xbox.GetLeftY(), 0);
break;
}
case(0x2):{ // keep going
//printf("middle \n");
myRobot.MecanumDrive_Cartesian(0, 0, -xbox.GetLeftY(), 0);
break;
}
case(0x4): // straif left
case(0x6):{
//printf("left \n");
float rightvalue = (fabs(xbox.GetLeftX())/2);
//printf("left leftstickX = %f\n", rightvalue);
myRobot.MecanumDrive_Cartesian(rightvalue, 0, -xbox.GetLeftY(), 0);
//myRobot.MecanumDrive_Cartesian(-leftstick.GetX(), -rightstick.GetX(), -leftstick.GetY(), 0);
break;
}
default:
break;
}
}
//void MecanumDrive_Cartesian(float x, float rotation, float y, float gyroAngle = 0.0);
if (!xbox.GetLeftBumper()){
myRobot.MecanumDrive_Cartesian(xbox.GetLeftX(), xbox.GetRightX(), -xbox.GetLeftY(), 0); //Mecanum drive at full speed
}
// forward/back
else{
myRobot.MecanumDrive_Cartesian(xbox.GetLeftX()/2, xbox.GetRightY()/2, xbox.GetLeftY()/2, 0);//Mecanumdrive at half speed
}
//Arm Code
//sf=shoulder front sensor
//sb=shoulder front sensor
//ef=shoulder front sensor
//eb=shoulder front sensor
//wf=shoulder front sensor
//wb=shoulder front sensor
//Shoulder Control
if(((!sfsensor->Get())||(armstick.GetY()<0)) || ((!sbsensor->Get())||(armstick.GetY()>0))) vicShoulder.Set(armstick.GetY()/2); //Shoulder Motor
//Elbow Control
if(armstick.GetRawButton(2)){
if((!efsensor->Get() && !ebsensor->Get())|| //Not either sensor
((efsensor->Get() && (armstick.GetY()<0))|| //on front sensor moveback only
((ebsensor->Get() && (armstick.GetY()>0))))) //on back sensor
vicElbow.Set(armstick.GetY()/2);
}
//Wrist Control
if(armstick.GetRawButton(3)){
if((!wfsensor->Get() && !wbsensor->Get())|| //Not either sensor
((wfsensor->Get() && (armstick.GetY()<0))|| //on front sensor moveback only
((wbsensor->Get() && (armstick.GetY()>0))))) //on back sensor
vicWrist.Set(armstick.GetY()/2);
}
//Claw Control
if(armstick.GetTrigger()){
vicClaw.Set(armstick.GetY()/2);
}
printf("gyro = %f\n", gyro.GetAngle());
Wait(0.005);// wait for a motor update time
}
}
};
START_ROBOT_CLASS(RobotDemo);
Safety captains be watchful. - Al Skierkiewicz [more] 
12-02-2011, 18:55
Linear Mode
