task main(){
initializeRobot();
// wait for start of tele-op phase
waitForStart();
//actual control of the robot
int _raw = 0;
int _processed = 0;
// Set the sensor to short range
HTEOPDsetShortRange(HTEOPD);
while (true){
getJoystickSettings(joystick);
//code for robot panning controlled by the left joystick
if(joystick.joy1_y1 > zero){ //forwards
ySet(joystick.joy1_y1);
}else if(joystick.joy1_y1 < -zero){ //backwards
ySet(joystick.joy1_y1);
}else if(joystick.joy1_x1 < -zero){ //left
xSet(joystick.joy1_x1);
}else if(joystick.joy1_x1 > zero){ //right
xSet(joystick.joy1_x1);
}else if(joystick.joy1_x2 < -zero){ //left turn
turn(joystick.joy1_x2);
}else if(joystick.joy1_x2 > zero){ //right turn
turn(joystick.joy1_x2);
}else{
ySet(0);
}
}
void turnDeg(float deg, float power){
if(deg > 0){
//currHeading -= ofset;
motor[leftBack] = power;
motor[rightBack] = -power;
while(deg >= currHeading){
wait1Msec(1);
nxtDisplayTextLine(2, "servo: %d", ServoValue[servo1]);
nxtDisplayTextLine(3, "head: %3.0f", currHeading);
nxtDisplayTextLine(5, "diff: %3.0f", SubtractFromCurrHeading (deg));
}
}else{
//currHeading += ofset;
motor[leftBack] = -power;
motor[rightBack] = power;
while(deg <= currHeading){
wait1Msec(1);
nxtDisplayTextLine(2, "servo: %d", ServoValue[servo1]);
nxtDisplayTextLine(3, "head: %3.0f", currHeading);
nxtDisplayTextLine(5, "diff: %3.0f", SubtractFromCurrHeading (deg));
}
}
xSet(0);
}
// Set table point nearest to some k
void
xTable::xSet(double x, double y, double value) {
x /= dx;
y /= dx;
int j = static_cast<int> (floor(x+0.5));
int i = static_cast<int> (floor(y+0.5));
xSet(i,j,value);
return;
}
void GoStraightByGyro (float straightHead, float power) {
// stay on heading from before button was pressed
if (SubtractFromCurrHeading(straightHead) > GyroTolerance) {
motor[rightBack] = power+ti;
motor[rightFront] = power+ti;
} else if (SubtractFromCurrHeading(straightHead) < -GyroTolerance) {
motor[leftBack] = power+ti;
motor[leftFront] = power+ti;
} else {
xSet(power);
}
}
//===========================================================================
cShape::cShape()
{
// INITIALIZE POSITION AND ORIENTATION:
pos = xSet(0.0, 0.0, 0.0);
rot = xIdentity33d();
// INITIALIZE COLOR (GRAY):
color = xSetColor4f(0.8, 0.8, 0.8); // Set red-green-blue components.
// INITIALIZE SPRING CONSTANT:
kSpring = 100; // units: [netwons per meter]
}
void forwardInc(float inches, float power){
nMotorEncoder[rightBack] = 0;
nMotorEncoder[leftBack] = 0;
int head = currHeading;
if(inches > 0){
while(nMotorEncoder[rightBack] < (inches/ROTDISTANCE)*360 || nMotorEncoder[leftBack] < (inches/ROTDISTANCE)*360){GoStraightByGyro(head, power);}
}else{
while(nMotorEncoder[rightBack] > (inches/ROTDISTANCE)*360 || nMotorEncoder[leftBack] > (inches/ROTDISTANCE)*360){GoStraightByGyro(head, -power);}
}
xSet(0);
}
//===========================================================================
cFinger::cFinger()
{
// INITIALIZE POSITION OF FINGER:
pos = xSet(0.0, 0.0, 0.0);
// INITIALIZE RADIUS:
radius = 0.003;
// INITIALIZE COLOR (RED DEFAULT COLOR):
color = xSetColor4f(1.0, 0.0, 0.0); // Set red-green-blue components.
// CLEAR FORCES:
clearForce();
// ENABLE FINGER FOR GRAPHIC RENDERING:
enable = true;
}
void turnDeg(float deg, float power) {
if(deg > 0) {
currHeading -= ofset;
motor[leftBack] = power;
motor[rightBack] = -power;
while(deg >= currHeading) {
wait1Msec(1);
}
} else {
currHeading += ofset;
motor[leftBack] = -power;
motor[rightBack] = power;
while(deg <= currHeading) {
wait1Msec(1);
}
}
xSet(0);
}
void forwardInc(float inches, float power){
nMotorEncoder[rightBack] = 0;
nMotorEncoder[leftBack] = 0;
//int head = currHeading;
if(inches > 0){
GoStraightByGyro(currHeading, power);
while(nMotorEncoder[rightBack] < (inches/ROTDISTANCE)*360 || nMotorEncoder[leftBack] < (inches/ROTDISTANCE)*360){
/*
if(head+4 < currHeading){
motor[rightBack] = power+ti;
motor[rightFront] = power+ti;
}else if(head-4 > currHeading){
motor[leftBack] = power-ti;
motor[leftFront] = power-ti;
}else{
xSet(power);
}
*/
}
}else{
GoStraightByGyro(currHeading, -power);
while(nMotorEncoder[rightBack] > (inches/ROTDISTANCE)*360 || nMotorEncoder[leftBack] > (inches/ROTDISTANCE)*360){
/*
if(head+4 < currHeading){
motor[rightBack] = power-ti;
motor[rightFront] = power-ti;
}else if(head-4 > currHeading){
motor[leftBack] = power+ti;
motor[leftFront] = power+ti;
}else{
xSet(power);
}
*/
}
}
xSet(0);
}
task main(){
initializeRobot();
// wait for start of tele-op phase
waitForStart();
liftCheck();
//actual control of the robot
while (true){
getJoystickSettings(joystick);
//code for robot panning controlled by the left joystick
if(joystick.joy1_y1 > zero){ //forwards
ySet(joystick.joy1_y1);
}else if(joystick.joy1_y1 < -zero){ //backwards
ySet(joystick.joy1_y1);
}else if(joystick.joy1_x1 < -zero){ //left
xSet(joystick.joy1_x1);
}else if(joystick.joy1_x1 > zero){ //right
xSet(joystick.joy1_x1);
}else if(joystick.joy1_x2 < -zero){ //left turn
turn(joystick.joy1_x2);
}else if(joystick.joy1_x2 > zero){ //right turn
turn(joystick.joy1_x2);
}else{
ySet(0);
}
//button actions
switch(joystick.joy1_Buttons){
case 1:
servo[backRightServo] = 50;
servo[backLeftServo] = 190;
break;
case 2:
servo[backRightServo] = 190;
servo[backLeftServo] = 30;
break;
case 3:
break;
case 4:
break;
case 5:
break;
case 6:
break;
case 7:
break;
case 8:
break;
default:
break;
}
if(joystick.joy1_TopHat != -1){
if(joystick.joy1_TopHat == 0){
motor[beaterBar] = 50;
}else if(joystick.joy1_TopHat == 1){
}else if(joystick.joy1_TopHat == 2){
}else if(joystick.joy1_TopHat == 3){
}else if(joystick.joy1_TopHat == 4){
motor[beaterBar] = 0;
}else if(joystick.joy1_TopHat == 6){
}else if(joystick.joy1_TopHat == 5){
}else if(joystick.joy1_TopHat == 7){
}
}
// the secound controller that does the 80-20 lifts and other attachments that are not the back goal clips
if(joystick.joy2_y1 > zero){ //increment up
}else if(joystick.joy2_y1 < -zero){ //increment down
}
}
}
//===========================================================================
void cFinger::clearForce()
{
force = xSet(0.0, 0.0, 0.0);
}
