void LSFindPosition_currentAsCentre_v2(int startDirection, int maxFindAngle, int speed) {
const int minSearchSpeed = 15;
if (startDirection == 0 ) return;
int direction = startDirection>0 ? 1 : -1;
LS_DirectionBeforeAligned = 0;
speed = abs(speed);
int _maxAngle, maxAngle = abs(maxFindAngle);
bool isFoundPosition = false;
int maxFoundVal = -1;
int maxLastTurnMax;
int nowVal;
if (speed <= minSearchSpeed) {
// still cannot find it. dont speed too much time on this.
writeDebugStreamLine("still cannot find it, exit");
return;
}
int _turn = 0;
while ( 1 ) {
_turn ++;
maxLastTurnMax = -1;
// reset the encoderVal to 0
nMotorEncoder[motorLS] = 0;
// the first turn, we make the turn half of the maxFindAngle.
if (_turn==1) {
_maxAngle = maxAngle / 2;
}else{
_maxAngle = maxAngle;
}
if (_turn>=9) {
writeDebugStreamLine("we speed too much turn, exit");
motor[motorLS] = 0;
break;
}
motor[motorLS] = direction * speed;
#define nowVal SensorValue[sensorLightLSRotate]
while ( nowVal < LSRotateLightThreshold ) {
if ( _turn<=2 ) {
// if this is the record turn
// record the max value
if (nowVal > maxFoundVal) {
maxFoundVal = nowVal;
writeDebugStreamLine("...updated maxVal==%d at turn%d", maxFoundVal, _turn);
}
} else {
// this is the finding turn
// then we stop at position where produced the max value, ...or
// just stop at the minCandidate value.
if (nowVal > maxFoundVal || nowVal > LSRotateLightCandidateThreshold ) {
motor[motorLS] = 0;
writeDebugStreamLine("get in with maxVal==%d at turn%d", maxFoundVal, _turn );
LSFindPosition_currentAsCentre(direction, maxFindAngle, speed);
return;
}
}
if(nowVal > maxLastTurnMax)
maxLastTurnMax = nowVal;
wait1Msec(msForMultiTasking);
if (abs(nMotorEncoder[motorLS]) > _maxAngle) {
// rotate too much?
break;
}
}
motor[motorLS] = 0;
wait1Msec(200);
if (SensorValue[sensorLightLSRotate] < LSRotateLightThreshold) {
// I think the rotation is just too much... rotate back
direction *= -1;
}else{
// we found the position!
writeDebugStreamLine("found it!exit");
LS_DirectionBeforeAligned = direction;
break;
}
writeDebugStreamLine("end of turn%d, with max=%d, speed=%d", _turn, maxLastTurnMax, speed);
// gradually decrease the speed.
speed = speed <= minSearchSpeed ? minSearchSpeed : speed-5;
}
#undef nowVal
}
void T(string t)
{
nxtScrollText(t);
writeDebugStreamLine(t);
}
int move( float dist, float power , int hold, int glide) //initially called with (-210 , 80, 1, 0)
{
float deg = (dist * DIST_SCALAR)/100; // scale from cm to motor rotations
int mult;
int lastTime, lastDegree;
if( dist > 0 ) // account for backwards or forwards
mult = 1;
else
mult = -1;
float timelimit = (abs(dist)/(power))*50; // set a time limit based on distance
nMotorEncoder[FrontR] = 0;
ClearTimer(T1);
lastTime = time100[T1];
lastDegree = nMotorEncoder[FrontR];
if(glide != 1)
{
while(abs(nMotorEncoder[FrontR]) < abs(deg))
{
if(time100[T1] > timelimit) // timeout if stalled
{
writeDebugStreamLine("timed out");
Stop(hold);
return 1;
}
if(lastDegree != nMotorEncoder[FrontR])
{
lastTime = time100[T1];
lastDegree = nMotorEncoder[FrontR];
}
else if(time100[T1] - lastTime >= 5)
{
writeDebugStreamLine("encoder problem");
Stop(hold);
return 1;
}
//writeDebugStreamLine(" encoder value: %d" , nMotorEncoder[FrontR]);
motor[FrontL] = power * mult;
motor[BackL] = power * mult;
motor[FrontR] = power * mult;
motor[BackR] = power * mult;
}
if(glide != -1)
{
motor[FrontL] = 0;
motor[BackL] = 0;
motor[FrontR] = 0;
motor[BackR] = 0;
}
return 0;
}
else
{
move(dist/3 , power , 1 , -1);
move(dist/3 , power/2 , 1 , -1);
move(dist/3 , power/3 , 1, 0);
return 0;
}
}
task main()
{
waitForStart();
//servoChangeRate[handJoint] = 10;
nMotorEncoder[spear] = 0;
writeDebugStreamLine("encoder set to: %d", nMotorEncoder[spear]);
int maxVal = 40;
while (true)
{
getJoystickSettings(joystick);
int cont1_left_yval = avoidWeird(joystick.joy1_y1, 20); //y coordinate for the left joystick on controller 1
int cont1_left_xval = avoidWeird(joystick.joy1_x1, 75); //x coordinate for the left joystick on controller 1
int cont1_right_yval = avoidWeird(joystick.joy1_y2, 20);
int cont1_dPad = joystick.joy1_TopHat; //Value of the dPad for controller 2
//if (joy1Btn(4) == 1)
//{
// if ((ServoValue[handJoint] + 5) < maxHandValue)
// {
// servo[handJoint] = ServoValue[handJoint] + 5;
// }
//}
//if (joy1Btn(2) == 1)
//{
// if ((ServoValue[handJoint] - 5) > minHandValue)
// {
// servo[handJoint] = ServoValue[handJoint] - 5;
// }
//}
if (joy1Btn(1) == 1){
nMotorEncoder[spear] = 0;
}
if (joy1Btn(4) == 1){
spearMovement(20);
}
if (joy1Btn(2) == 1){
spearMovement(-20);
}
if (joy1Btn(2) != 1 && joy1Btn(4) != 1){
spearMovement(0);
}
//if (joy1Btn(6) == 1)
//{
// fold_arm(false);
//}
//if (joy1Btn(5) == 1)
//{
// fold_arm(true);
//}
if (joy1Btn(1) == 1){
maxVal = 100;
}
if (joy1Btn(1) != 1){
maxVal = 40;
}
//if (joy2Btn(1) == 1)
//{
// servo[ramp] = 0;
//}
//if (joy2Btn(3) == 1)
//{
// servo[ramp] = 255;
//}
//if (joy2Btn(1) != 1 && joy2Btn(3) != 1)
//{
// servo[ramp] = 128;
//}
drive(cont1_left_yval, cont1_left_xval, maxVal);
//shoulderMovement(cont1_right_yval);
//handMovement(cont1_dPad);
}
}
task main()
{
initializeRobot();
clearTimer(T1);
bool isLaunching = false;
bool isDropping = false;
int mThreshold = 15; // Sets dead zones to avoid unnecessary movement
int aThreshold = 30;
int xVal, yVal; //Stores left analog stick values
float scaleFactor = 40.0 / 127; //Sets max. average motor power and maps range of analog stick to desired range of power
//waitForStart(); // wait for start of tele-op phase
int num = 0;
while (true)
{
getJoystickSettings(joystick); // Retrieves data from the joystick
//4 is forward, 2 is backwards, 7 is left, 8 is right
if(joy1Btn(4) == 1){
if(num == 4) {
motor[motorD] = 20;
motor[motorE] = 20;
} else {
finishAction(num);
num = 4;
}
} else if (joy1Btn(2) == 1){
if(num == 2) {
motor[motorD] = -23;
motor[motorE] = -23;
} else {
finishAction(num);
num = 2;
}
} else if (joy1Btn(1) == 1){
if(num == 1) {
turn(1);
} else {
finishAction(num);
num = 1;
}
}else if (joy1Btn(3) == 1){
if(num == 3) {
turn(-1);
} else {
finishAction(num);
num = 3;
}
//raise scissor lift (positive motor power)
/*
} else if (joy1Btn(9) == 1){
if(num == 9 && flag9) {
flag9 = false;
releaseBallCollector();
//
} else {
finishAction(num);
num = 9;
}
*/
/*} else if (joy1Btn(6) == 1){
if(num == 13 && flag13) {
flag13 = false;
raiseGrabber();
//
} else {
finishAction(num);
num = 13;
}//raise grabber
} else if (joy1Btn(7) == 1){
if(num == 14 && flag14) {
flag14 = false;
lowerGrabber();
//
} else {
finishAction(num);
num = 14;
}
//lower grabber */
}else if (joy1Btn(7) == 1){
clearDebugStream();
writeDebugStreamLine(path);
stopAllTasks();
}else{
if(num != 0) {
finishAction(num);
num = 0;
}
//RECORD CURRENT VARIABLES, THEN RESET EVERYTHING
}
//Controls launcher
//if(joy1Btn(1) == 1 && isLaunching == false){
//isLaunching = true;
//fireLauncher();
//isLaunching = false;
//}
//wait1Msec(1);
}
}
task main ()
{
ubyte type;
ubyte ID;
ubyte state;
ubyte value;
string dataString;
string tmpString;
// initialise the port, etc
RS485initLib();
startTask(updateScreen);
// Disconnect if already connected
N2WDisconnect();
N2WchillOut();
sleep(1000);
if (!N2WCustomExist())
{
stopTask(updateScreen);
sleep(50);
eraseDisplay();
playSound(soundException);
displayCenteredBigTextLine(1, "ERROR");
displayTextLine(3, "No custom profile");
displayTextLine(4, "configured!!");
while(true) sleep(1);
}
N2WLoad();
sleep(100);
N2WConnect(true);
connStatus = "connecting";
while (!N2WConnected()) sleep(100);
sleep(1000);
connStatus = "connected";
playSound(soundBeepBeep);
sleep(3000);
N2WgetIP(IPaddress);
sleep(1000);
// 123456789012345
dataStrings[0] = "Tch | Snr | Clr";
// on | 011 | 1"
while (true)
{
if (N2WreadWS(type, ID, state, value))
{
writeDebugStreamLine("btn: %d, state: %d", ID, state);
switch (ID)
{
case 1: doStraight(state); break;
case 3: doRight(state); break;
case 4: doStop(state); break;
case 5: doLeft(state); break;
case 7: doReverse(state); break;
case 9: doAction(state); break;
case 11: handleColour(state); break;
default: break;
}
}
// All values are only updated when they've changed.
// This cuts backs drastically on the number of messages
// that have to be sent to the NXT2WIFI
// Fetch the state of the Touch Sensor
// This value is displayed by field 0 (in0) on the page
currTouchState = SensorBoolean[TOUCH];
if (currTouchState != prevTouchState)
{
memset(data, 0, sizeof(data));
data[0] = (currTouchState) ? '1' : '0';
N2WwriteWS(1, 0, data, 2);
prevTouchState = currTouchState;
N2WchillOut();
}
// Fetch the currently detected colour.
// This value is displayed by field 1 (in1) on the page
currDetectedColour = SensorValue[COLOUR];
if (currDetectedColour != prevDetectedColour)
{
sprintf(dataString, "%d", currDetectedColour);
memcpy(data, dataString, strlen(dataString));
N2WwriteWS(1, 1, data, strlen(dataString));
prevDetectedColour = currDetectedColour;
N2WchillOut();
}
// Fetch the distance detected by the sonar sensor
// This value is displayed by field 2 (in2) on the page
currSonarDistance = SensorValue[SONAR];
if (currSonarDistance != prevSonarDistance)
{
sprintf(dataString, "%d", currSonarDistance);
memcpy(data, dataString, strlen(dataString));
N2WwriteWS(1, 2, data, strlen(dataString));
prevSonarDistance = currSonarDistance;
N2WchillOut();
}
// Fetch the tacho count for motor A
// This value is displayed by field 3 (in3) on the page
currEncMotorA = nMotorEncoder[MOT_ACTION];
if (currEncMotorA != prevEncMotorA)
{
sprintf(dataString, "%d", currEncMotorA);
memcpy(data, dataString, strlen(dataString));
N2WwriteWS(1, 3, data, strlen(dataString));
prevEncMotorA = currEncMotorA;
N2WchillOut();
}
// Fetch the tacho count for motor B
// This value is displayed by field 4 (in4) on the page
//currEncMotorB = nMotorEncoder[MOT_LEFT];
if (currEncMotorB != prevEncMotorB)
{
sprintf(dataString, "%d", currEncMotorB);
memcpy(data, dataString, strlen(dataString));
N2WwriteWS(1, 4, data, strlen(dataString));
prevEncMotorB = currEncMotorB;
N2WchillOut();
}
// Fetch the tacho count for motor C
// This value is displayed by field 5 (in5) on the page
currEncMotorC = nMotorEncoder[MOT_RIGHT];
if (currEncMotorC != prevEncMotorC)
{
sprintf(dataString, "%d", currEncMotorC);
memcpy(data, dataString, strlen(dataString));
N2WwriteWS(1, 5, data, strlen(dataString));
prevEncMotorC = currEncMotorC;
N2WchillOut();
}
// Fetch the current voltage level. The average one
// works best, the other one jumps around too much.
// This value is displayed by field 6 (in6) on the page
currBatteryLevel = nAvgBatteryLevel;
if (currBatteryLevel != prevBatteryLevel)
{
sprintf(dataString, "%d", currBatteryLevel);
memcpy(data, dataString, strlen(dataString));
N2WwriteWS(1, 6, data, strlen(dataString));
prevBatteryLevel = currBatteryLevel;
N2WchillOut();
}
sprintf(dataStrings[2], "A: %d", currEncMotorA);
sprintf(dataStrings[3], "B: %d", currEncMotorB);
sprintf(dataStrings[4], "C: %d", currEncMotorC);
sprintf(tmpString, "%s | %3d", (currTouchState == 0) ? "off" : "on ", currSonarDistance);
sprintf(dataStrings[1], "%s | %3d", tmpString, currDetectedColour);
}
}
void parseInput()
{
writeDebugStreamLine("Beging parsing...");
ubyte currByte[] = {0};
ubyte prevByte[] = {0};
ubyte conn[] = {0};
int cid;
string tmpString;
int index = 0;
time1[T1] = 0;
while (true)
{
if (time1[T1] > 300000)
return;
alive();
if (nxtGetAvailHSBytes() > 0)
{
nxtReadRawHS(&currByte[0], 1);
if ((prevByte[0] == 27) && (currByte[0] == 'F')) {
return;
} else if ((prevByte[0] == 27) && (currByte[0] == 'S')) {
index = 0;
memset(rxbuffer, 0, sizeof(rxbuffer));
wait1Msec(1);
nxtReadRawHS(&conn[0], 1);
cid = conn[0] - 48;
writeDebugStreamLine("Conn: %d", cid);
if (cid > 1)
return;
while (true) {
if (time1[T1] > 300000)
return;
while (nxtGetAvailHSBytes() == 0) EndTimeSlice();
nxtReadRawHS(&currByte[0], 1);
if ((prevByte[0] == 27) && (currByte[0] == 'F')) {
return;
} else if ((prevByte[0] == 27) && (currByte[0] == 'E')) {
rxbuffer[index--] = 0;
rxbuffer[index--] = 0;
PlaySound(soundShortBlip);
while(bSoundActive) EndTimeSlice();
break;
}
prevByte[0] = currByte[0];
rxbuffer[index++] = currByte[0];
if (index == sizeof(rxbuffer))
return;
}
StringFromChars(tmpString, &rxbuffer[0]);
writeDebugStreamLine(tmpString);
/*
for (int i = 0; i < ((index / 19) + 1); i++) {
memset(BytesRead[0], 0, 20);
memcpy(BytesRead[0], rxbuffer[i*19], 19);
StringFromChars(tmpString, BytesRead);
writeDebugStream(tmpString);
}*/
genResponse(cid);
}
prevByte[0] = currByte[0];
}
}
}
void InitializeDebugStream()
{
writeDebugStreamLine("===============");
}
task main ()
{
int index;
// port to use for the socket
int BOFHport = 6666;
string dataString;
// get our bluetooth name
getFriendlyName(dataString);
int avail = 0;
nNxtButtonTask = -2;
StartTask(updateScreen);
// initialise the port, etc
RS485initLib();
memset(RS485rxbuffer, 0, sizeof(RS485rxbuffer));
memset(RS485txbuffer, 0, sizeof(RS485txbuffer));
N2WchillOut();
// Disconnect if already connected
N2WDisconnect();
N2WchillOut();
// if a custom profile exists, use that instead
if (N2WCustomExist())
{
N2WLoad();
}
else
{
// enable DHCP
N2WsetDHCP(true);
wait1Msec(100);
// Disable adhoc
N2WsetAdHoc(false);
wait1Msec(100);
// SSID to connect to
N2WsetSSID("YOURWIFINETWORK");
wait1Msec(100);
// The passphrase to use
N2WSecurityWPA2Passphrase("YOURWIFIPASSPHRASE");
wait1Msec(100);
// Save this profile to the custom profile
N2WSave();
wait1Msec(100);
// Load the custom profile
N2WLoad();
}
wait1Msec(100);
N2WConnect(true);
connStatus = "connecting";
while (!N2WConnected())
wait1Msec(1000);
connStatus = "connected";
PlaySound(soundBeepBeep);
wait1Msec(3000);
N2WgetIP(IPaddress);
wait1Msec(1000);
// Open a listening socket on port 6666
if (!N2WTCPOpenServer(1, BOFHport))
{
writeDebugStreamLine("Err open port %d", BOFHport);
PlaySound(soundException);
while(bSoundActive) EndTimeSlice();
StopAllTasks();
}
while (true)
{
// Check if anyone has sent us anything
avail = N2WTCPAvail(1);
if (avail > 0)
{
rxbytes += avail;
N2WchillOut();
PlaySound(soundFastUpwardTones);
// Read what the client sent us
sprintf(dataStrings[0], "%d bytes from", avail);
N2WTCPRead(1, avail);
N2WchillOut();
// Get the MAC address of the client
dataStrings[2] = "Remote MAC:";
N2WTCPClientMAC(1, dataStrings[3]);
writeDebugStream("MAC: ");
writeDebugStreamLine(dataStrings[3]);
N2WchillOut();
// check the IP address of the client
N2WTCPClientIP(1, dataStrings[1]);
N2WchillOut();
// Send back a hearty "Hi there, <IP>!"
index = 0;
returnMsg = "Hi there, ";
index = RS485appendToBuff(buffer, index, returnMsg);
index = RS485appendToBuff(buffer, index, dataStrings[1]);
returnMsg = "\n";
index = RS485appendToBuff(buffer, index, returnMsg);
txbytes += index;
N2WTCPWrite(1, (tHugeByteArray)buffer, index);
N2WchillOut();
// Terminate the connection to the client
N2WTCPDetachClient(1);
N2WchillOut();
}
// Wait a bit
wait1Msec(50);
}
}
task main()
{
clearDebugStream();
nMotorEncoder[car] = 0;
while(1)
{
getJoystickSettings(joystick);
writeDebugStreamLine("encoder value: %d" , nMotorEncoder[car] );
if(joy1Btn(10))
nMotorEncoder[car] = 0;
if(joy1Btn(4))
{
writeDebugStreamLine("down");
//servo[sweeperServo] = 200;
motor[car] = -90;
}
else if(joy1Btn(2))
{
writeDebugStreamLine("up");
//servo[sweeperServo] = 100;
motor[car] = 90;
}
else
{
//servo[sweeperServo] =127;
//motor[deploySweep] = 0;
motor[car] = 0;
//motor[elevator] = 0;
}
//if(joy1Btn(1))
// motor[pin] = 100;
//else if(joy1Btn(3))
// motor[pin] = 100;
//else
// motor[pin] = 0;
//getEvents();
//for(int i = 0; i<eventCnt;i++)
//{
// switch(bevents[i].keyID){
// case JOY1_YELLOW :
// if(bevents[i].state == B_PRSD)
// motor[elevator] = 10;
// else if(bevents[i].state == B_RLSD)
// motor[elevator] = 0;
// break;
// case JOY1_GREEN:
// if(bevents[i].state == B_PRSD)
// motor[elevator] = -10;
// else if(bevents[i].state == B_RLSD)
// motor[elevator] = 0;
// break;
// };
//}
//_________________________________________________________________________
}
}
task main()
{
initializeRobot();
// waitForStart(); // Wait for the beginning of autonomous phase.
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
//// ////
//// Add your robot specific autonomous code here. ////
//// ////
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
// CONFIGURATION VARIABLES //
int driveTime = 3000; //in MILLISECONDS 1000 MS = 1 second //
int bucketDump = 1600;
int bucketStop = 800;
int L_Flip_Open = 15;
int L_Flip_Close = 150;
int R_Flip_Close = 85;
int R_Flip_Open = 230;
///////////////////////////////////////////////////////
// BEGIN AUTONOMOUS ROUTINE //
///////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
// BEGIN IR ROUTINE
/////////////////////////////////////////////////////////////
int _dirAC = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
int maxSig = 0; // the max signal strength from the seeker.
int val = 0; // the translated directional value.
// we are going to set DSP mode to 1200 Hz.
tHTIRS2DSPMode _mode = DSP_1200;
// attempt to set to DSP mode.
if (HTIRS2setDSPMode(HTIRS2, _mode) == 0)
{
// unsuccessful at setting the mode.
// display error message.
eraseDisplay();
nxtDisplayCenteredTextLine(0, "ERROR!");
nxtDisplayCenteredTextLine(2, "Init failed!");
nxtDisplayCenteredTextLine(3, "Connect sensor");
nxtDisplayCenteredTextLine(4, "to Port 1.");
// make a noise to get their attention.
PlaySound(soundBeepBeep);
// wait so user can read message, then leave main task.
wait10Msec(300);
return;
}
eraseDisplay();
// loop continuously and read from the sensor.
while(true)
{
// read the current modulated signal direction
_dirAC = HTIRS2readACDir(HTIRS2);
if (_dirAC < 0)
{
// error! - write to debug stream and then break.
writeDebugStreamLine("Read dir ERROR!");
break;
}
// Get the AC signal strength values.
if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
{
// error! - write to debug stream and then break.
writeDebugStreamLine("Read sig ERROR!");
break;
} else {
// find the max signal strength of all detectors.
maxSig = (acS1 > acS2) ? acS1 : acS2;
maxSig = (maxSig > acS3) ? maxSig : acS3;
maxSig = (maxSig > acS4) ? maxSig : acS4;
maxSig = (maxSig > acS5) ? maxSig : acS5;
}
// display info
nxtDisplayCenteredBigTextLine(1, "Dir=%d", _dirAC);
nxtDisplayCenteredBigTextLine(4, "Sig=%d", maxSig);
// figure out which direction to go...
// a value of zero means the signal is not found.
// 1 corresponds to the far left (approx. 8 o'clock position).
// 5 corresponds to straight ahead.
// 9 corresponds to far right.
// first translate directional index so 0 is straight ahead.
val = _dirAC - 5;
// calculate left and right motor speeds.
motor[Left_drive] = 30 * val;
motor[Right_drive] = 30 * val;
// wait a little before resuming.
wait10Msec(2);
}
/////////////////////////////////////////////////////////////
// END IR ROUTINE
/////////////////////////////////////////////////////////////
/*
// Drive forward for drivetime @ 20% power //
motor[Left_drive] = -20;
motor[Right_drive] = -20;
wait1Msec(driveTime);
motor[Left_drive] = 0;
motor[Right_drive] = 0;
// Set Flippers to Open //
servo[Left_Flipper] = L_Flip_Open;
servo[Right_Flipper] = R_Flip_Open;
wait1Msec(2000);
// Rotate clockwise for 1000ms @ 20% power //
motor[Left_drive] = -20;
motor[Right_drive] = 20;
wait1Msec(2000);
motor[Left_drive] = 0;
motor[Right_drive] = 0;
// Perform lift cycle //
nMotorEncoderTarget[bucket] = bucketDump; // target bucket dumping position
motor[bucket] = -25;
while(nMotorRunState[bucket] != runStateIdle) // While Motor is still running, allow to go to target posititon
{
// Do not continue.
}
motor[bucket] = 0;
// if(SensorValue(touchBucketZero)== 0)
// {
nMotorEncoderTarget[bucket] = bucketStop; // target bucket drive position
motor[bucket] = 25;
while(nMotorRunState[bucket] != runStateIdle) // While Motor is still running, allow to go to target posititon
{
// Do not continue.
}
motor[bucket] = 0;
// Set Flippers to Close //
servo[Left_Flipper] = L_Flip_Close;
servo[Right_Flipper] = R_Flip_Close;
wait1Msec(1000);
// Rotate counterclockwise for 1000ms @ 20% power //
motor[Left_drive] = -20;
motor[Right_drive] = 20;
wait1Msec(2000);
motor[Left_drive] = 0;
motor[Right_drive] = 0;
// Drive forward for 1000ms = drivetime/3 @ 20% power //
motor[Left_drive] = 20;
motor[Right_drive] = 20;
wait1Msec(driveTime/3);
motor[Left_drive] = 0;
motor[Right_drive] = 0;
// Rotate counterclockwise for 500ms @ 20% power //
motor[Left_drive] = -20;
motor[Right_drive] = 20;
wait1Msec(2000);
motor[Left_drive] = 0;
motor[Right_drive] = 0;
// Drive forward for 3500ms @ 20% power //
motor[Left_drive] = 40;
motor[Right_drive] = 40;
wait1Msec(2500);
motor[Left_drive] = 0;
motor[Right_drive] = 0;
////////////////////////////
// End Autonomous Routine //
////////////////////////////
*/
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
servo[handJoint] = 240;
wait1Msec(100);
black_threshold = LSvalNorm(middle_light);
white_threshold = black_threshold + 5;
writeDebugStreamLine("black_threshold: %d", black_threshold);
writeDebugStreamLine("white_threshold: %d", white_threshold);
//
// Move to other side of field
//
moveStraight(50,600);
move(-50,50,650);
moveStraight(50,1300);
move(50,-50,350);
moveStraight(50,450);
//
// Find the line
//
// Continue forward until the middle sensor detects the white line
//
writeDebugStreamLine("middle light sensor value going into first loop: %d" , LSvalNorm(middle_light));
while (LSvalNorm(middle_light) < white_threshold){
//!--Code for debugging
middle_nrm = LSvalNorm(middle_light);
if (middle_nrm != temp_middle_nrm) {
writeDebugStreamLine("middle light sensor value: %d", middle_nrm);
temp_middle_nrm = middle_nrm;
}
//--!
setAllMotorVals(20);
}
allStop();
//
// Turn 90 degrees
//
// Move forward a little more and then turn until the middle sensor detects the white line
//
bool isLeft = true; // If this boolean is true, the 90 degree turn is toward the left
moveStraight(30,350);
//moveStraight(15,1000); // with long arm
// Make a variable that counts the number of times through the loop. If it goes above a certain
// threshold, assume the robot is stuck on the edge of the wood and increase the power
int counter1 = 0;
while (LSvalNorm(left_light) < white_threshold){
//!--Code for debugging
left_nrm = LSvalNorm(left_light);
if (left_nrm != temp_left_nrm) {
writeDebugStreamLine("left light sensor value: %d", middle_nrm);
temp_left_nrm = left_nrm;
}
//--!
if (isLeft){
if (counter1 > 500){
setDriveMotorVals(60,-60);
}
else{
setDriveMotorVals(40,-40);
}
}
else{
if (counter1 > 100){
setDriveMotorVals(-20,20);
}
else{
setDriveMotorVals(-15,15); // with long arm
}
}
counter1 += 1;
}
//
// Follow the line until touch sensor is triggered
//
// 1. Left on white, middle on white (or black), right on black: turn toward the left
// 2. Left on black, middle on white (or black), right on white: turn toward the right
// 3. Left on black, middle on white, right on black: move straight
// 4. Left on black, middle on black, right on black: lost...
//
// Set up touch sensor variables
int nButtonsMask;
bool isTouch = false; // Boolean indicating if a touch sensor has been pressed.
// If one has, this changes to true.
// Make a variable that counts the number of times through the loop. If it goes above a certain
// threshold, assume the robot is stuck on the edge of the wood and increase the power
int counter2 = 0;
//deploy the spear
deploySpear(true);
while (isTouch == false){
writeDebugStreamLine("-----------------counter2 val: %d", counter2);
// Read light sensor values
left_nrm = LSvalNorm(left_light);
middle_nrm = LSvalNorm(middle_light);
right_nrm = LSvalNorm(right_light);
//!--Code for debugging
//if (left_nrm != temp_left_nrm) {
writeDebugStreamLine("left light sensor value: %d", left_nrm);
//temp_left_nrm = left_nrm;
//}
//if (middle_nrm != temp_middle_nrm) {
writeDebugStreamLine("middle light sensor value: %d", middle_nrm);
//temp_middle_nrm = middle_nrm;
//}
//if (right_nrm != temp_right_nrm) {
writeDebugStreamLine("middle light sensor value: %d", right_nrm);
//temp_right_nrm = right_nrm;
//}
//--!
if (left_nrm < black_threshold){
// left sensor is black
if (middle_nrm < black_threshold){
// middle sensor is black
if (right_nrm > black_threshold){
// right sensor is white or gray
// turn left
writeDebugStreamLine("Case 1: turned left");
if (counter2 > 600){
setDriveMotorVals(30,0);
}
else{
setDriveMotorVals(20,0);
}
}
else{
// right sensor is black
// lost, so just turn left a lot
setDriveMotorVals(-30,30);
}
}
else{
// middle sensor is not black
if (middle_nrm > white_threshold){
// middle sensor is white
// assume right sensor is black
// move straight
writeDebugStreamLine("Case 2: went straight");
if (counter2 > 600){
setAllMotorVals(30);
}
else{
setAllMotorVals(20);
}
}
else{
// middle sensor is gray
// assume right sensor is gray
// turn left
writeDebugStreamLine("Case 3: turned left");
if (counter2 > 600){
setDriveMotorVals(30,0);
}
else{
setDriveMotorVals(20,0);
}
}
}
}
else{
// left sensor is not black
if (left_nrm > white_threshold){
// left sensor is white
// assume middle sensor is black
// assume the right sensor is black too
// turn right
writeDebugStreamLine("Case 4: turned right");
if (counter2 > 600){
setDriveMotorVals(0,30);
}
else{
setDriveMotorVals(0,20);
}
}
else{
// left sensor is gray
// middle sensor is gray
// assume right sensor is black
// turn right
writeDebugStreamLine("Case 5: turned right");
if (counter2 > 600){
setDriveMotorVals(0,30);
}
else{
setDriveMotorVals(0,20);
}
}
}
counter2 += 1;
// Read touch sensor values
nButtonsMask = SensorValue[S3];
if (nButtonsMask & 0x01)
isTouch = true;
if (nButtonsMask & 0x02)
isTouch = true;
}
allStop();
wait1Msec(1000);
//
// More later....scoring stuff
//
// 1. Move backward a little
// 2. Unfold arm
// 3. Move forward a little
// 4. Move arm down to score
// 5. Back up
// 6. Reset arm
// 7. Move toward our rings?
//
// back up a little
moveStraight(-20,800);
// unfold arm
fold_arm(false);
// move forward a little
moveStraight(20,600);
// move arm down
motor[shoulderJoint] = -50;
wait1Msec(800);
motor[shoulderJoint] = 0;
// back up
moveStraight(-40,500);
// reset arm
fold_arm(true);
//retract spear
//deploySpear(false);
while (true){
return;
}
}
task main()
//task testSubtask()
{
//Display NXT
nxtDisplayCenteredTextLine(0, "PMX robot-test");
nxtDisplayCenteredBigTextLine(1, "MOTOR");
nxtDisplayCenteredTextLine(3, "pmSquareTest");
writeDebugStreamLine("PMX robot-test | MOTOR | pmSquareTest");
//float dist = 1.000; // in meters
float x1, y1, theta1;
TRAJ_STATE ts;
motion_Init();
pid_ConfigKP(motors[ALPHA_DELTA][ALPHA_MOTOR].PIDSys, 800);
pid_ConfigKI(motors[ALPHA_DELTA][ALPHA_MOTOR].PIDSys, 0);
pid_ConfigKD(motors[ALPHA_DELTA][ALPHA_MOTOR].PIDSys, 400);
pid_ConfigDPeriod(motors[ALPHA_DELTA][ALPHA_MOTOR].PIDSys, 3);
pid_ConfigKP(motors[ALPHA_DELTA][DELTA_MOTOR].PIDSys, 500);
pid_ConfigKI(motors[ALPHA_DELTA][DELTA_MOTOR].PIDSys, 0);
pid_ConfigKD(motors[ALPHA_DELTA][DELTA_MOTOR].PIDSys, 500);
pid_ConfigDPeriod(motors[ALPHA_DELTA][DELTA_MOTOR].PIDSys, 3);
pid_ConfigKP(motors[LEFT_RIGHT][LEFT_MOTOR].PIDSys, 75);
pid_ConfigKI(motors[LEFT_RIGHT][LEFT_MOTOR].PIDSys, 0);
pid_ConfigKD(motors[LEFT_RIGHT][LEFT_MOTOR].PIDSys, 50);
pid_ConfigKP(motors[LEFT_RIGHT][RIGHT_MOTOR].PIDSys, 75);
pid_ConfigKI(motors[LEFT_RIGHT][RIGHT_MOTOR].PIDSys, 5);
pid_ConfigKD(motors[LEFT_RIGHT][RIGHT_MOTOR].PIDSys, 50);
motors_ConfigAllIMax(90000);
ts = motion_DoLine(-0.10);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
/*
ts = motion_DoRotate(PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoLine(-0.10);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoRotate(PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoLine(-0.10);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoRotate(PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoLine(-0.10);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoRotate(PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
*/
ts = motion_DoRotate(PI / 2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
/*
ts = motion_DoRotate(-(PI / 2.0));
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoRotate(PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoRotate(-PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoRotate(PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoRotate(-PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoRotate(PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoRotate(-PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
*/
Sleep(1000);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x1=%f y1=%f theta1=%f",x1, y1, theta1);
writeDebugStreamLine("pmSquareTest.c : END OF PMX !!!!");
motion_StopTimer();
}
void LSRotateToPosition(int motorName, int direction, int maxFindAngle=40*tRatioLS) {
if (direction == 0 ) return;
direction = direction>0 ? 1 : -1;
const int minSearchSpeed = 25;
int speed = 55;
int _maxAngle, maxAngle = abs(maxFindAngle);
bool isFirstTime=true;
int nTurn = 0;
// this is for the test proprose.
LSFindPosition_currentAsCentre(direction, maxFindAngle, 55);
return;
while (1) {
// reset the encoderVal to 0
nMotorEncoder[motorLS] = 0;
writeDebugStreamLine("\n + at turn %d (%d)", nTurn, nTurn%2==0);
nTurn++;
motor[motorName] = direction * speed;
while ( SensorValue[sensorLightLSRotate] < LSRotateLightThreshold) {
// the first turn, we make the turn half of the maxFindAngle.
if (isFirstTime) {
_maxAngle = maxAngle / 2;
isFirstTime = false;
}else{
_maxAngle = maxAngle;
}
// TODO: in every turn, we record the maxim value,
// in next turn, we should at least turn to that value
// (reliability promise)
writeDebugStream("%d, ", SensorValue[sensorLightLSRotate]);
wait1Msec(msForMultiTasking);
if (abs(nMotorEncoder[motorLS]) > _maxAngle) {
// rotate too much?
break;
}
}
motor[motorName] = 0;
wait1Msec(200);
if (SensorValue[sensorLightLSRotate] < LSRotateLightThreshold) {
// I think the rotation is just too much... rotate back
direction *= -1;
if (speed <= minSearchSpeed) {
// still cannot find it. dont speed too much time on this.
writeDebugStreamLine("cannot found it");
break;
}
// gradually decrease the speed.
speed = speed <= minSearchSpeed ? minSearchSpeed : speed-7;
}else{
writeDebugStreamLine("found it");
break;
}
}
// TODO: continue to turn(small), find the largest val
}
int initialize() // sets a zero point for the begining
{
int average = HTGYROstartCal(HTGYRO);
writeDebugStreamLine("average: %d", average);
return average;
}
void print_closest()
{
int min_idx = find_closest_sonar_bin();
writeDebugStreamLine("closest is at %d deg, distance of %d mm", min_idx * 360/SONAR_BINS, sonar_readings[min_idx]);
}
//Moving function: enter coordinates and the robot does the rest. Keeps track of position by time-based dead reckoning.
//Calibration while running a longer program is possible by assigning negative x- and y-coordinates (the robot will push itself into a corner of the field),
//and then running calibrate (6)
void slitherto(float xgoal, float ygoal, float rgoal) {
float alfa;
float beta;
float _x1 = 0;
float _y1 = 0;
float _x1s = 0;
float _y1s = 0;
float dirx;
float diry;
bool atgoal = false;
float distancecm;
float betacor;
float spd = 30;
float _x2 = 0;
float rotspeed;
atgoal = !((dirx == 1 && xgoal > xcur) || (dirx == -1 && xgoal < xcur) || (diry == 1 && ygoal > ycur) || (diry == -1 && ygoal < ycur) || round(rcur) != rgoal);
beta = atan2(ygoal-ycur,xgoal-xcur);
betacor = beta + degreesToRadians(rcur);
_x1 = cos(betacor);
_y1 = sin(betacor);
dirx = sgn(xgoal-floor(xcur));
diry = sgn(ygoal-floor(ycur));
time1[T1] = 0;
while(atgoal == false) {
{
if ((dirx == 1 && xgoal > xcur) || (dirx == -1 && xgoal < xcur)) {
_x1s = 1;
}
else {
_x1s = 0;
}
if ((diry == 1 && ygoal > ycur) || (diry == -1 && ygoal < ycur)) {
_y1s = 1;
}
else {
_y1s = 0;
}
if (round(rcur) < rgoal - 5 && (_x1s || _y1s))
_x2 = 1;
else if (round(rcur) > rgoal + 5 && (_x1s || _y1s))
_x2 = -1;
else if (round(rcur) < rgoal && (_x1s || _y1s))
_x2 = 0.3;
else if (round(rcur) > rgoal && (_x1s || _y1s))
_x2 = -0.3;
else if (round(rcur) < rgoal && !(_x1s || _y1s))
_x2 = 1;
else if (round(rcur) > rgoal && !(_x1s || _y1s))
_x2 = -1;
if (beta/1.571 - floor(beta/1.571) > 0.785) //Oh radians....
alfa = 1.571 - (beta/1.571 - floor(beta/1.571));
else
alfa = beta/1.571 - floor(beta/1.571);
wait1Msec(10);
distancecm = abs(((1-C_D)/(-0.785*alfa) + 1)*((d0_1s_1+d0_1s_2)/2)*C_T*time1[T1]*0.001);
xcur += distancecm*cos(beta);
ycur += distancecm*sin(beta);
HTACreadAllAxes(HTAC, xacc, yacc, zacc);
xacc -= xcal;
yacc -= ycal;
zacc -= zcal;
writeDebugStreamLine("%d,%d",xacc,yacc);
rotspeed = HTGYROreadRot(gyro); //Read the current rotation speed
rcur += rotspeed * time1[T1]*0.001; //Magic
nxtDisplayCenteredBigTextLine(3, "%2.0f", rcur); //Display our current heading on the screen
if (abs(xacc) > 120 || abs(yacc) > 120) {
PlaySound(soundBeepBeep);
//Move in opposite direction
motor[fr] = spd*(_x1*_x1s-_y1*_y1s);
motor[br] = spd*(-_x1*_x1s-_y1*_y1s);
motor[bl] = spd*(-_x1*_x1s+_y1*_y1s);
motor[fl] = spd*(_x1*_x1s+_y1*_y1s);
time1[T1] = 0;
while (time1[T1] < 1000) {
time1[T2] = 0;
wait1Msec(10);
rotspeed = HTGYROreadRot(gyro); //Read the current rotation speed
rcur += rotspeed * time1[T2]*0.001; //Magic
nxtDisplayCenteredBigTextLine(3, "%2.0f", rcur);
}
motor[fr] = 0;
motor[br] = 0;
motor[bl] = 0;
motor[fl] = 0;
distancecm = abs(((1-C_D)/(-0.785*alfa) + 1)*((d0_1s_1+d0_1s_2)/2)*C_T*time1[T1]*0.001);
xcur += distancecm*cos(-beta);
ycur += distancecm*sin(-beta);
wait1Msec(1000);
}
motor[fr] = spd*(-_x1*_x1s+_y1*_y1s-_x2);
motor[br] = spd*(_x1*_x1s+_y1*_y1s-_x2);
motor[bl] = spd*(_x1*_x1s-_y1*_y1s-_x2);
motor[fl] = spd*(-_x1*_x1s-_y1*_y1s-_x2);
atgoal = !((dirx == 1 && xgoal > xcur) || (dirx == -1 && xgoal < xcur) || (diry == 1 && ygoal > ycur) || (diry == -1 && ygoal < ycur) || round(rcur) != rgoal);
time1[T1] = 0; //Reset timer
}
motor[fr] = 0;
motor[br] = 0;
motor[bl] = 0;
motor[fl] = 0;
}
}
task sonar_scanner()
{
nMotorEncoder[sonar_rotate] = 0;
pid_controller_t sonar_pid_controller;
init_sonar_pid_controller(&sonar_pid_controller);
pid_state_t sonar_pid_state;
init_pid_state(&sonar_pid_state, &sonar_pid_controller);
int sonar_destination = 0;
reset_readings();
while (1)
{
// keep moving the sonar
// turn the robot towards the closest object
long encoder_reading = nMotorEncoder[sonar_rotate];
long sonar_min = 0;
long sonar_max = 720;
sonar_pid_controller.MAX_OUTPUT = MAX_SONAR_MOTOR_SPEED;
int closest_bin = find_closest_sonar_bin();
switch (sonar_scan_mode)
{
case Sonar_Full:
if (sonar_readings[closest_bin] < 500 && have_recent_sonar_readings())
{
sonar_scan_mode = Sonar_Close;
}
else
{
sonar_min = 0;
sonar_max = 720;
if (sonar_destination != sonar_min && sonar_destination != sonar_max)
sonar_destination = sonar_min;
}
break;
case Sonar_Close:
if (sonar_readings[closest_bin] >= 500 || !have_recent_sonar_readings())
{
sonar_scan_mode = Sonar_Full;
}
else
{
sonar_min = (closest_bin - 2) * ENCODER_COUNTS_PER_BIN + 1;
sonar_max = (closest_bin + 2) * ENCODER_COUNTS_PER_BIN;
if (sonar_destination != sonar_min && sonar_destination != sonar_max)
sonar_destination = sonar_min;
}
break;
}
if (encoder_reading > sonar_min - sonar_pid_controller.CLOSE_ENOUGH && encoder_reading < sonar_min + sonar_pid_controller.CLOSE_ENOUGH)
sonar_destination = sonar_max;
else if (encoder_reading > sonar_max - sonar_pid_controller.CLOSE_ENOUGH && encoder_reading < sonar_max + sonar_pid_controller.CLOSE_ENOUGH)
sonar_destination = sonar_min;
writeDebugStreamLine("using sonar_destination: %d", sonar_destination);
long output = update_pid_controller(&sonar_pid_controller, &sonar_pid_state, sonar_destination, nMotorEncoder[sonar_rotate]);
motor[sonar_rotate] = output;
int reading = SensorValue[ultrasonic];
//writeDebugStreamLine("got sonar reading: %d", reading);
const int MIN_SONAR_DISTANCE = 30;
if (reading > MIN_SONAR_DISTANCE)
{
int bin = mod(encoder_reading / ENCODER_COUNTS_PER_BIN, SONAR_BINS);
sonar_readings[bin] = reading;
sonar_times[bin] = nPgmTime;
}
wait1Msec(1);
}
}
task autostraighten() {
while(1) {
if(vexRT[Ch3] >= 120 && vexRT[Ch2] >= 120 || true) { //if the user is going about full speed and nearly straight
userDriveControl = false; //disable user drivetrain control so we can auto-straighten
//reset encoders
nMotorEncoder[rDriveFront] = 0;
nMotorEncoder[lDriveFront] = 0;
int straighteningError,
slewRateLimit = 15;
float power = 127,
lPower,
rPower,
lPowerLast = vexRT[Ch3], //set the initial last power values to whatever the joystick is when auto-straighten is activated
rPowerLast = vexRT[Ch2];
while (vexRT[Ch3] >= 60 && vexRT[Ch2] >= 60 || true) {
//adjust the powers sent to each side if the encoder values don't match
straighteningError = nMotorEncoder[lDriveFront] - nMotorEncoder[rDriveFront];
//if (straighteningError > 0) { //left side is ahead, so speed up the right side or slow down the left side
// lPower = power + straighteningError*straighteningKpLeft;
// //if (rPower > 127) { //if the correction we would apply
// // rPower = power;
// // lPower = power - straighteningError*straighteningKpLeft;
// //}
//} else { //otherwise, just set the right side to the power
// lPower = power;
//}
//if (straighteningError < 0) { //right side is ahead, so speed up the left side
// rPower = power - straighteningError*straighteningKpRight;
// //if (lPower > 127) {
// // lPower = power;
// // rPower = power + straighteningError*straighteningKpRight;
// //}
//} else { //otherwise, just set the left side to the power
// lPower = power;
//}
if (straighteningError > 0) { //left side is ahead, so speed up the right side
lPower = power - straighteningError*straighteningKpLeft;
} else { //otherwise, just set the right side to the power
lPower = power;
}
if (straighteningError < 0) { //right side is ahead, so speed up the left side
rPower = power + straighteningError*straighteningKpRight;
} else { //otherwise, just set the right side to the power
rPower = power;
}
//update last motor powers with new ones
lPowerLast = lPower;
rPowerLast = rPower;
//send new motor powers;
setLDriveMotors(lPower);
setRDriveMotors(rPower);
writeDebugStreamLine("%d,%f,%f",nPgmTime, lPower, rPower);
wait1Msec(25);
}
userDriveControl = true;
}
wait1Msec(25);
}
}
task main()
{
pid_controller_t left_pid_controller;
init_drive_pid_controller(&left_pid_controller);
pid_controller_t right_pid_controller;
init_drive_pid_controller(&right_pid_controller);
reset_readings();
while (1)
{
check_and_switch_mode();
switch (robot_mode)
{
case Do_Nothing:
stop_moving();
motor[sonar_rotate] = 0;
StopTask(sonar_scanner);
break;
case Turn:
turn_left();
break;
case Follow_Closest_Object:
// keep moving the sonar
StartTask(sonar_scanner);
// turn the robot towards the closest object
int closest_bin = find_closest_sonar_bin();
writeDebugStreamLine("closest_bin: %d", closest_bin);
int degrees_to_turn = closest_bin * (360/SONAR_BINS);
//writeDebugStreamLine("degrees_to_turn: %d", degrees_to_turn);
while (degrees_to_turn > 180)
{
degrees_to_turn -= 360;
}
while (degrees_to_turn < -180)
{
degrees_to_turn += 360;
}
//writeDebugStreamLine("degrees_to_turn: %d", degrees_to_turn);
if (degrees_to_turn < 360/SONAR_BINS)
{
move_motor_to_position(right,
nMotorEncoder[right] + RIGHT_MOTOR_ENCODING_TICKS_PER_DEGREE * degrees_to_turn,
&right_pid_controller);
//turn_left();
}
else if (degrees_to_turn > 360/SONAR_BINS)
{
move_motor_to_position(left,
nMotorEncoder[left] + LEFT_MOTOR_ENCODING_TICKS_PER_DEGREE * degrees_to_turn,
&left_pid_controller);
//turn_right();
}
else
{
go_straight();
}
wait1Msec(1);
break;
default:
return;
}
}
}
void genResponse(int cid) {
int power = motor[motorA];
float temp = 0.0;
string tmpString;
int index = 0;
ubyte linebuff[20];
StringFromChars(tmpString, rxbuffer);
index = StringFind(tmpString, "/");
StringDelete(tmpString, 0, index);
index = StringFind(tmpString, "HTTP");
StringDelete(tmpString, index, strlen(tmpString));
writeDebugStreamLine("Request:%s", tmpString);
nxtDisplayTextLine(2, "Request: ");
nxtDisplayTextLine(3, tmpString);
if (StringFind(tmpString, "MOTA") > 0) {
StringDelete(tmpString, 0, 6);
index = StringFind(tmpString, " ");
if (index > -1)
StringDelete(tmpString, index, strlen(tmpString));
//power = RC_atoix(tmpString);
power = clip(atoi(tmpString), -100, 100);
writeDebugStreamLine("Power:%d", power);
} else {
writeDebugStreamLine("NO POWER: %s", tmpString);
}
sendHeader(cid);
while(nxtHS_Status == HS_SENDING) wait1Msec(5);
wait1Msec(100);
index = 0;
linebuff[0] = 27; // escape;
linebuff[1] = 'S'; // the CID;
linebuff[2] = (ubyte)cid + 48; // the CID;
index = RS485appendToBuff(RS485txbuffer, index, linebuff, 3);
StringFormat(tmpString, "MotorA=%d\n", power);
memcpy(linebuff, tmpString, strlen(tmpString));
index = RS485appendToBuff(RS485txbuffer, index, linebuff, strlen(tmpString));
DTMPreadTemp(DTMP, temp);
StringFormat(tmpString, "Temp: %2.2f C", temp);
memcpy(linebuff, tmpString, strlen(tmpString));
index = RS485appendToBuff(RS485txbuffer, index, linebuff, strlen(tmpString));
linebuff[0] = 27; // escape;
linebuff[1] = 'E'; // the CID;
index = RS485appendToBuff(RS485txbuffer, index, endmarker, 2);
RS485write(RS485txbuffer, index);
if (power != 0) nMotorEncoderTarget[motorA] = 2000;
motor[motorA] = power;
if (power > 0)
SensorType[COLOUR] = sensorCOLORGREEN;
else if (power < 0)
SensorType[COLOUR] = sensorCOLORBLUE;
else if (nMotorRunState[motorA] == runStateIdle)
SensorType[COLOUR] = sensorCOLORRED;
else
SensorType[COLOUR] = sensorCOLORRED;
wait1Msec(300);
RS485clearRead();
closeConn(1);
memset(RS485rxbuffer, 0, sizeof(RS485rxbuffer));
//wait1Msec(100);
RS485read(RS485rxbuffer, sizeof(RS485rxbuffer));
//clear_read_buffer();
}
task usercontrol()
{
// User control code here, inside the loop
int potThreshold = 1616;
int flipperVal = 0;
//clearDebugStream;
//ClearTimer(T1);
wait1Msec(2000); // wait 2 seconds before exectuing following code
bMotorReflected[port2] = true;
while (true)
{
// This is the main execution loop for the user control program. Each time through the loop
// your program should update motor + servo values based on feedback from the joysticks.
// .....................................................................................
// Insert user code here. This is where you use the joystick values to update your motors, etc.
// .....................................................................................
int ch2 = vexRT[Ch2];
int ch3 = vexRT[Ch3];
int secondch2 = vexRT[Ch2Xmtr2];
int secondch3 = vexRT[Ch3Xmtr2];
int pot = SensorValue[potentiometer];
//motor[motor1] = motor[motor2] = motor[motor3] = motor[motor10] = ch2;
// Flipper on Controller 2
/*motor[flip1] = secondch2;
motor[flip2] = secondch2;
motor[flip3] = secondch2;
motor[flip4] = secondch2;*/
//Flipper button control
if(vexRT[Btn6UXmtr2])
{
motor[servo] = -125;
wait1Msec(500);
motor[servo] = 95;
}
motor[flip1] = flipperVal;
motor[flip2] = flipperVal;
motor[flip3] = flipperVal;
motor[flip4] = flipperVal;
if(vexRT[Btn8DXmtr2] && pot < potThreshold) {
flipperVal = -127;
} else if (vexRT[Btn8UXmtr2]) {
flipperVal = 127;
}else if (pot > potThreshold) {
flipperVal = -20;
} else {
flipperVal = 0;
}
// doesn't work yet
/*if(vexRT[Btn8DXmtr2] && pot < potThreshold) {
flipperVal = -127;
} else if (vexRT[Btn8DXmtr2] && pot > potThreshold) {
flipperVal = -20;
} else if (pot > potThreshold && !vexRT[Btn8DXmtr2]) {
if (pot > 750) {
flipperVal = 127;
} else {
flipperVal = 0;
}}*/
// Drive On Controller 1
motor[backLeft] = ch3;
motor[frontLeft] = ch3;
motor[backRight] = ch2;
motor[frontRight] = ch2;
if(vexRT[Btn7UXmtr2])
{
motor[intake] = -127;
}
if(vexRT(Btn7DXmtr2))
{
motor[intake] = 0;
}
if(vexRT[Btn5U]) {
motor[backLeft] = 127;
motor[frontLeft] = -127;
motor[backRight] = -127;
motor[frontRight] = 127;
}
if(vexRT[Btn6U]) {
motor[backLeft] = -127;
motor[frontLeft] = 127;
motor[backRight] = 127;
motor[frontRight] = -127;
}
//int pot = SensorValue[potentiometer];
//sfoo = time100[T1]/10;
writeDebugStreamLine("The potentiometer is reading %d", SensorValue[potentiometer]);
//int shoot vexRT[Btn8D];
clearLCDLine(0); // clear the top VEX LCD line
clearLCDLine(1); // clear the bottom VEX LCD line
setLCDPosition(0,0); // set the VEX LCD cursor the first line, first space
displayNextLCDString("Potentiometer:"); // display "Potentiometer:" on the top line
setLCDPosition(1,0); // set the VEX LCD cursor the second line, first space
displayNextLCDNumber(SensorValue(potentiometer));
}
}
task main()
{
// Variables
long encnow = 0;
float rpmconversion = 21*((60.0)/392.0);
// Reset the encoder value to zero
resetMotorEncoder(InsideMotor);
SensorValue[I2C_1] = 0;
// Clear the debug window
clearDebugStream();
while(true)
{
// Iterate through the different power levels 0 to -128
for(int i = -1; i >= -128; i--)
{
// Set the motor powers
motor[InsideMotor] = i;
motor[OutsideMotor] = i;
// Wait one second and then get the encoder value
wait(1, seconds);
encnow = SensorValue[I2C_1];
// Read the encoder value and output the sensor value and rpm calculation
writeDebugStreamLine("%d\t%d\t%f", i, encnow, (rpmconversion*encnow));
// Reset the ticks of the sensor
SensorValue[I2C_1] = 0;
if(i == -128)
{
for(int x = -128; x < 0; x = x + 20)
{
motor[InsideMotor] = x;
motor[OutsideMotor] = x;
wait(500,milliseconds);
}
motor[InsideMotor] = 0;
motor[OutsideMotor] = 0;
wait(1,seconds);
SensorValue[I2C_1] = 0;
}
}
// Iterate through the different power levels, 0 to 127
for(int i = 0; i <= 127; i++)
{
// Set the motor powers
motor[InsideMotor] = i;
motor[OutsideMotor] = i;
// Wait one second and then get the encoder value
wait(1, seconds);
encnow = SensorValue[I2C_1];
// Read the encoder value and output the sensor value and rpm calculation
writeDebugStreamLine("%d\t%d\t%f", i, encnow, (rpmconversion*encnow));
// Reset the ticks of the sensor
SensorValue[I2C_1] = 0;
if(i == 127)
{
for(int x = 0; x > 0; x = x - 20)
{
motor[InsideMotor] = x;
motor[OutsideMotor] = x;
wait(500,milliseconds);
}
motor[InsideMotor] = 0;
motor[OutsideMotor] = 0;
wait(1,seconds);
SensorValue[I2C_1] = 0;
}
}
}
}
// main task
task main ()
{
int _dirAC1 = 0;
int _dirAC2 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
int acS12, acS22, acS32, acS42, acS52 = 0;
int maxSig = 0; // the max signal strength from the seeker.
int maxSig2 = 0;
// we are going to set DSP mode to 1200 Hz.
tHTIRS2DSPMode _mode = DSP_1200;
// attempt to set to DSP mode.
if (HTIRS2setDSPMode(HTIRS2, _mode) == 0 || HTIRS2setDSPMode(ir2, _mode) == 0)
{
// unsuccessful at setting the mode.
// display error message.
eraseDisplay();
nxtDisplayCenteredTextLine(0, "ERROR!");
nxtDisplayCenteredTextLine(2, "Init failed!");
nxtDisplayCenteredTextLine(3, "Connect sensor");
nxtDisplayCenteredTextLine(4, "to Ports 1 and 4.");
// make a noise to get their attention.
PlaySound(soundBeepBeep);
// wait so user can read message, then leave main task.
wait10Msec(300);
return;
}
eraseDisplay();
// loop continuously and read from the sensor.
while(true)
{
// read the current modulated signal direction
_dirAC1 = HTIRS2readACDir(HTIRS2);
_dirAC2 = HTIRS2readACDir(ir2);
if (_dirAC1 < 0)
{
// error! - write to debug stream and then break.
writeDebugStreamLine("Read dir ERROR!");
break;
}
if (_dirAC2 < 0)
{
// error! - write to debug stream and then break.
writeDebugStreamLine("Read dir ERROR!");
break;
}
// Get the AC signal strength values.
if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
{
// error! - write to debug stream and then break.
writeDebugStreamLine("Read sig ERROR!");
break;
} else {
// find the max signal strength of all detectors.
maxSig = (acS1 > acS2) ? acS1 : acS2;
maxSig = (maxSig > acS3) ? maxSig : acS3;
maxSig = (maxSig > acS4) ? maxSig : acS4;
maxSig = (maxSig > acS5) ? maxSig : acS5;
}
if (!HTIRS2readAllACStrength(ir2, acS12, acS22, acS32, acS42, acS52 ))
{
// error! - write to debug stream and then break.
writeDebugStreamLine("Read sig ERROR!");
break;
} else {
// find the max signal strength of all detectors.
maxSig2 = (acS12 > acS22) ? acS12 : acS22;
maxSig2 = (maxSig2 > acS32) ? maxSig2 : acS32;
maxSig2 = (maxSig2 > acS42) ? maxSig2 : acS42;
maxSig2 = (maxSig2 > acS52) ? maxSig2 : acS52;
}
// display info
nxtDisplayCenteredTextLine(1, "DirL = %d", _dirAC1);
nxtDisplayCenteredTextLine(4, "DirR = %d", _dirAC2);
nxtDisplayCenteredTextLine(2, "SigL = %d", maxSig);
nxtDisplayCenteredTextLine(5, "SigR = %d", maxSig2);
// figure out which direction to go...
// a value of zero means the signal is not found.
// 1 corresponds to the far left (approx. 8 o'clock position).
// 5 corresponds to straight ahead.
// 9 corresponds to far right.
// first translate directional index so 0 is straight ahead.
// wait a little before resuming.
wait10Msec(2);
}
}
task Foreground() {
//servoChangeRate[servo2] = 2;
Pid_Init1();
Pid_Init2();
int timeLeft;
int state=0;
int speedCmd = 0;
int dirCmd = 0;
servo[irArm]=105;
const tMUXSensor LEGOUS = msensor_S4_3;
while(true) {
ClearTimer(T1);
hogCPU();
//--------------------Robot Code---------------------------//
long armEncoder = nMotorEncoder[blockthrower];
long robotDist = nMotorEncoder[rtMotor] + nMotorEncoder[ltMotor];
long robotDir = nMotorEncoder[ltMotor] - nMotorEncoder[rtMotor];
long distInches = robotDist/IN2CLK;
long dirDegrees = robotDir/DEG2CLK;
// Calculate the speed and direction commands
if(shortPathTrue == false)
{
speedCmd = ForwardDist(path[pathIdx][DIST_IDX], robotDist, path[pathIdx][SPD_IDX]);
dirCmd=Direction(path[pathIdx][DIR_IDX], robotDir);
}
else
{
speedCmd = ForwardDist(shortPath[pathIdx][DIST_IDX], robotDist, shortPath[pathIdx][SPD_IDX]);
dirCmd=Direction(shortPath[pathIdx][DIR_IDX], robotDir);
}
int armSpd = FlipperArm(armEncoder, armSetPos);
bool IRval;
bool SonarVal;
//calculate when to increment path
if (abs(path[pathIdx][DIR_IDX] - dirDegrees) < 4 && abs(path[pathIdx][DIST_IDX] - distInches) < 3) pathIdx++;
// State O Follow Path
if (state==0)
{
if (distInches>18)
{
state=1;
}
}
IRval = Delayatrue(1, SensorValue[IR] == 4 || SensorValue[IR] == 3);
// State 1 Look for IR Beacon
if (state==1)
{
speedCmd=10;
if ( IRval)
{
state=7;
}
else
{
state=2;
}
}
// State 2 Follow Path
if (state==2)
{
if (distInches>27)
{
state=3;
}
}
// State 3 Look for IR Beacon
if (state==3)
{
speedCmd=10;
if ( IRval==true)
{
state=7;
}
else
{
state=4;
}
}
// State 4 Follow Path
if (state==4)
{
if (distInches>46)//36
{
state=5;
}
}
// State 5 Look for IR Beacon
if (state==5)
{
speedCmd=10;
if ( IRval==true)
{
state=7;
}
else
{
state=6;
}
}
// State 6 Follow Path
if (state==6)
{
if (pathIdx == 1)//45
{
state=7;
}
}
if (state==7)// flip arm
{
speedCmd=0;
dirCmd = 0;
armSetPos = 1900;
if (abs(armSetPos - armEncoder) <20)
{
state=8;
}
servo[irArm]=243;
}
if (state==8)
{
speedCmd = 0;
dirCmd = 0;
armSetPos = 0;
if (abs(armSetPos) - abs(armEncoder) < 200)
{
state=9;
}
}
SonarVal = Delayatrue2(1, USreadDist(LEGOUS) > 40);
if (state==9)
{
if ((distInches>90) && (distInches<115))
{
if(SonarVal)
{
state=10;
}
else
{
state=11;
}
}
}
if(state==10)
{
shortPathTrue=true;
}
if(state==11)
{
}
/*
DebugInt("spd",speedCmd);
DebugInt("dir",robotDir/DEG2CLK);
DebugInt("sonarval",SonarVal);
DebugInt("path",pathIdx);
DebugInt("state",state);
DebugInt("dist",distInches);
DebugInt("ir", SensorValue[IR]);
*/
writeDebugStreamLine("i,pthIdx,rbtDist,irval,spdCmd,IR,state, rightencoder, leftencoder");
writeDebugStreamLine("%3i,%3i,%4i,%4i,%3i,%3i,%3i, %4i, %4i",iFrameCnt,pathIdx,distInches,IRval,speedCmd,SensorValue[IR],state, nMotorEncoder[rtMotor], nMotorEncoder[ltMotor]);
// Calculate when to move to the next path index
int s=sizeof(path)/sizeof(path[0])-1;
DebugInt("siz",s);
if (pathIdx>s) pathIdx=s;
speedCmd = RateLimit(speedCmd, START_RATE,speedCmdZ1 );
rightmotors=speedCmd-dirCmd;
leftmotors=speedCmd+dirCmd;
motor[rtBack]=rightmotors;
motor[rtMotor]=rightmotors;
motor[ltMotor]=leftmotors;
motor[ltBack]=leftmotors;
motor[blockthrower]=armSpd;
DebugInt("rightmotors",rightmotors);
DebugInt("leftmotors",leftmotors);
//--------------------------Robot Code--------------------------//
// Wait for next itteration
timeLeft=FOREGROUND_MS-time1[T1];
releaseCPU();
wait1Msec(timeLeft);
}// While
}//Foreground
//positive powers will go forward or right
//negative powers values go backwards or left
//encoder counts is how many counts to go, always positive
//power is the power to run the motors at before straightening is applied
//time is a maximum time to complete the operation
void driveDistance(int power, int encoderCounts, int direction) {
//writeDebugStreamLine("nPgmTime,error,nMotorEncoder[lDriveFront], nMotorEncoder[rDriveFront],pTerm,iTerm,dTerm,lPower,rPower");
//reset encoder values
SensorValue[lDriveEnc] = 0;
SensorValue[rDriveEnc] = 0;
int straighteningError = 0;
float lPower,
rPower;
time1[T1] = 0;
if (direction == STRAIGHT || direction == STRAFE_LEFT || direction == STRAFE_RIGHT) { //validate direction
//limit the values of the power term to only be those that can be taken by the motors
if (power > 127) {
power = 127;
} else if (power < -127) {
power = -127;
}
if (direction == STRAIGHT) {
while(encoderCounts > abs(SensorValue[lDriveEnc] + SensorValue[rDriveEnc])/2.0) {
//adjust the powers sent to each side if the encoder values don't match
straighteningError = SensorValue[lDriveEnc] - SensorValue[rDriveEnc];
if (straighteningError > 0) { //left side is ahead, so slow it down
lPower = power - straighteningError*straighteningKpLeft;
} else { //otherwise, just set the right side to the power
lPower = power;
}
if (straighteningError < 0) { //right side is ahead, so slow it down
rPower = power + straighteningError*straighteningKpRight;
} else { //otherwise, just set the right side to the power
rPower = power;
}
writeDebugStreamLine("lPower = %d, rPower = %d,lDriveEnc = %d, rDriveEnc = %d",lPower,rPower,SensorValue[lDriveEnc],SensorValue[rDriveEnc]);
setLeftDtMotorsSeparate(lPower,lPower);
setRightDtMotorsSeparate(rPower,rPower);
wait1Msec(25);
}
setLeftDtMotorsSeparate(0,0);
setRightDtMotorsSeparate(0,0);
} else if (direction == STRAFE_LEFT) {
while(encoderCounts > (abs(SensorValue[lDriveEnc]) + abs(SensorValue[rDriveEnc]))/2.0) {
//adjust the powers sent to each side if the encoder values don't match
straighteningError = abs(SensorValue[lDriveEnc]) - abs(SensorValue[rDriveEnc]);
if (straighteningError > 0) { //left side is ahead, so slow it down
lPower = power - straighteningError*straighteningKpLeft*-1;
} else { //otherwise, just set the right side to the power
lPower = power;
}
if (straighteningError < 0) { //right side is ahead, so slow it down
rPower = power + straighteningError*straighteningKpRight*-1;
} else { //otherwise, just set the right side to the power
rPower = power;
}
setLeftDtMotorsSeparate(lPower*-1,lPower*1);
setRightDtMotorsSeparate(rPower*1,rPower*-1);
wait1Msec(25);
}
//writeDebugStreamLine("%d,%f,%f,%f,%f,%f,%f,%f,%f",nPgmTime,target,error,nMotorEncoder[lDriveFront], nMotorEncoder[rDriveFront],pTerm,iTerm,dTerm,lPower,rPower);
setLeftDtMotorsSeparate(0,0);
setRightDtMotorsSeparate(0,0);
} else if (direction == STRAFE_RIGHT) {
while(encoderCounts > (abs(SensorValue[lDriveEnc]) + abs(SensorValue[rDriveEnc]))/2.0) {
//adjust the powers sent to each side if the encoder values don't match
straighteningError = abs(SensorValue[lDriveEnc]) - abs(SensorValue[rDriveEnc]);
//writeDebugStreamLine("%d,%d,%d,%d",lPower*lfMult,lPower*lbMult,rPower*rfMult,rPower*rbMult);
//positive powers:
// if left side is ahead, straightening error is positive
// lPower will decrease
// if right side is ahead, straightening error is negative
// rPower will decrease
//negative powers:
// if left side is ahead, straightening error is positive
// power is negative, straighteningError is positive
// power would go higher (more negative) because power - (positive error)
// fix: multiply by sign of power - after this change
// power is positive, straighteningError is still positive (as we want)
// power is negative, straighteningError is multiplied by -1, so:
// (-127) - (12)*(.3)*-1 = -127 + 4 = -123, slower power
//
if (straighteningError > 0) { //left side is ahead, so slow it down
lPower = power - straighteningError*straighteningKpLeft;
} else { //otherwise, just set the right side to the power
lPower = power;
}
if (straighteningError < 0) { //right side is ahead, so slow it down
rPower = power + straighteningError*straighteningKpRight;
} else { //otherwise, just set the right side to the power
rPower = power;
}
setLeftDtMotorsSeparate(lPower*1,lPower*-1);
setRightDtMotorsSeparate(rPower*-1,rPower*1);
wait1Msec(25);
}
//writeDebugStreamLine("%d,%f,%f,%f,%f,%f,%f,%f,%f",nPgmTime,target,error,nMotorEncoder[lDriveFront], nMotorEncoder[rDriveFront],pTerm,iTerm,dTerm,lPower,rPower);
setLeftDtMotorsSeparate(0,0);
setRightDtMotorsSeparate(0,0);
}
}
}
task main()
{
gyroCal();
//waitForStart();
clearDebugStream();
writeDebugStreamLine("starting!");
nMotorEncoder[FrontR] = 0;
nMotorEncoder[grabber1] = 0;
nMotorEncoder[grabber2] = 0;
int beac = getSeeker();
if(beac == -1)
{
writeDebugStreamLine("This no good");
Stop(0);
}
else if(beac == 1)
{
writeDebugStreamLine("orientation 1 is running");
turn(-41, 25);
move(50, 30, 1, 0);
turn(82, 25);
move (40, 40, 1, 0);
}
else if(beac == 2)
{
writeDebugStreamLine("orientation 2 is running");
turn(-19, 25);
move(60, 30, 1, 0);
turn(30, 25);
move(40, 40, 1, 0);
}
else if(beac == 3)
{
writeDebugStreamLine("orientation 3 is running");
move(60, 30, 1, 0);
turn(-36, 25);
move(40, 40, 1, 0);
}
else
{
writeDebugStreamLine("the sensor thought it was in region %d", beac);
}
//move(-10 , 30, 1, 1);
//writeDebugStreamLine("finished enitial move");
//wait10Msec(100);
//turn(82, 25);
//writeDebugStreamLine("finished first turn");
/*move(-40 , 50, 1, 0);
turn(-45, 50);
move(-10, 30, 1, 0);
ClearTimer(T1);
while(nMotorEncoder[grabber1] > -90 && time1[T1] < 1500)
{
if(abs(nMotorEncoder[grabber1]-nMotorEncoder[grabber2]) > 10)
writeDebugStreamLine("the grabbers aren't moving with each other");
motor[grabber1] = -30;
motor[grabber2] = -30;
}
motor[grabber1] = 0;
motor[grabber2] = 0;*/
}
task main () {
int X = 0;
memset(data, 0, sizeof(data));
TIRresetSensor(TIR);
nxtDisplayCenteredTextLine(0, "Dexter Industries");
nxtDisplayCenteredTextLine(1, "Thermal Infrared");
nxtDisplayCenteredTextLine(3, "Test 1");
nxtDisplayCenteredTextLine(5, "Connect sensor");
nxtDisplayCenteredTextLine(6, "to S1");
wait1Msec(2000);
eraseDisplay();
// set emissivity for light skin
TIRsetEmissivity(TIR, TIR_EM_SKIN_LIGHT);
nMotorEncoderTarget[VERTICAL] = 200;
motor[VERTICAL] = -20;
while((nMotorRunState[VERTICAL] != runStateIdle) && (nMotorRunState[VERTICAL] != runStateHoldPosition)) EndTimeSlice();
nMotorEncoderTarget[HORIZONTAL] = 360;
motor[HORIZONTAL] = 20;
while((nMotorRunState[HORIZONTAL] != runStateIdle) && (nMotorRunState[HORIZONTAL] != runStateHoldPosition)) EndTimeSlice();
wait1Msec(500);
nMotorEncoder[HORIZONTAL] = 0;
nMotorEncoder[VERTICAL] = 0;
PlaySound(soundBeepBeep);
while(bSoundActive) EndTimeSlice();
for (int i = 0; i < 80; i++) {
wait1Msec(500);
X = 0;
memset(data, 0, sizeof(data));
nMotorEncoderTarget[HORIZONTAL] = 720;
motor[HORIZONTAL] = -40;
time1[T1] = 0;
while((nMotorRunState[HORIZONTAL] != runStateIdle) && (nMotorRunState[HORIZONTAL] != runStateHoldPosition)) {
data[X] = TIRreadObjectTemp(TIR);
X++;
wait1Msec(20);
}
nxtDisplayBigTextLine(1, "X: %d", X);
nxtDisplayBigTextLine(3, "T: %d", time1[T1]);
nMotorEncoderTarget[VERTICAL] = 5;
motor[VERTICAL] = 20;
nMotorEncoderTarget[HORIZONTAL] = 720;
motor[HORIZONTAL] = 60;
for (int j = 0; j < 200; j++) {
if (data[j] != 0) {
writeDebugStream("%3.2f,", data[j]);
wait1Msec(5);
}
}
writeDebugStreamLine("");
while((nMotorRunState[HORIZONTAL] != runStateIdle) && (nMotorRunState[HORIZONTAL] != runStateHoldPosition))
EndTimeSlice();
}
bFloatDuringInactiveMotorPWM = true;
wait1Msec(10);
}
task main(){
//--------------------INIT Code---------------------------//
ForwardDistReset((tMotor)rtMotor, (tMotor)ltMotor);
DirectionReset();
nMotorEncoder[blockthrower] = 0;
speedCmdZ1=0;
pathIdx=0;
delayatruecount=0;
int state=0;
Pid_Init1();
Pid_Init2();
//--------------------End INIT Code--------------------------//
waitForStart(); // Wait for the beginning of autonomous phase.
int iFrameCnt=0;
int timeLeft;
servo[irArm]=243;
while(true){
ClearTimer(T1);
hogCPU();
//--------------------Robot Code---------------------------//
long armEncoder = nMotorEncoder[blockthrower];
long robotDist = nMotorEncoder[rtMotor] + nMotorEncoder[ltMotor];
long robotDir = nMotorEncoder[ltMotor] - nMotorEncoder[rtMotor];
long distInches = robotDist/IN2CLK;
long dirDegrees = robotDir/27;
// Calculate the speed and direction commands
int speedCmd = ForwardDist(path[pathIdx][DIST_IDX], robotDist, path[pathIdx][SPD_IDX]);
int dirCmd = Direction(path[pathIdx][DIR_IDX], robotDir);
int armSpd = FlipperArm(armEncoder, armSetPos);
bool IRval;
//calculate when to increment path
if (abs(path[pathIdx][DIR_IDX] - dirDegrees) < 4 && abs(path[pathIdx][DIST_IDX] - distInches) < 3) pathIdx++;
// Calculate the IR value
IRval = Delayatrue(1, SensorValue[IR] == 5 || SensorValue[IR] == 6);
if (state==0)// State O Follow Path
{
if (distInches>28)
{
state=1;
}
}
if(state==1)// State 1 Swing out irArm
{
servo[irArm]=150;
if (distInches>34)
{
state=2;
}
}
if (state==2)// state 2 look for ir under box 1
{
if ( IRval||SensorValue[IR] == 3||SensorValue[IR] == 2)
{
state=12;
servo[irArm]=243;
}
else
{
state=3;
}
}
if (state==12)//follows path before flipping arm
{
//speedCmd=0;
if(distInches>44)
{
state = 8;
}
}
if (state==3)//state 3 look for box 2 and follow path
{
if (distInches>46)
{
state=4;
}
}
if (state==4)//state 4 look for ir under box 2
{
if ( IRval==true||SensorValue[IR] == 3)
{
state=13;
servo[irArm]=243;
}
else
{
state=15;
}
servo[irArm]=243;
}
if (state==13) // waits for distance before flipping
{
if(distInches>57)
{
state = 8;
}
}
if (state==15) // pulls servo arm out
{
if(distInches>57)
{
servo[irArm] = 80;
state =5;
}
}
if (state==5)//state 5 look for box 3 and follow path
{
if (distInches>66)//36
{
state=6;
}
}
if (state==6)// State 6 Look for ir under box 3
{
if ( IRval||SensorValue[IR] == 4||SensorValue[IR] == 3||SensorValue[IR] == 2)
{
state=14;
}
else
{
state=7;
}
servo[irArm]=243;
}
if (state==14)// waits distance before flipping arm
{
if(distInches>69)
state = 8;
}
if (state==7)// State 7 look for box 4
{
if (pathIdx == 3)//45
{
state=8;
}
}
if (state==8)// state 8 flip arm
{
servo[irArm]=243;
speedCmd=0;
dirCmd = 0;
armSetPos = 2300;
if (abs(armSetPos - armEncoder) <10)
{
state=9;
}
}
if (state==9)//state 9 lower arm
{
speedCmd = 0;
dirCmd = 0;
armSetPos = 0;
if (abs(armSetPos - armEncoder) < 400)
{
pathIdx=3;
state=10;
}
}
if(state==10)//state 10 follow path
{
}
//DebugInt("spd",speedCmd);
//DebugInt("dir",robotDir/DEG2CLK);
//DebugInt("dist",distInches);
//DebugInt("path",pathIdx);
//DebugInt("state",state);
DebugInt("irval",SensorValue[IR]);
// Calculate when to move to the next path index
int s=sizeof(path)/sizeof(path[0])-1;
if (pathIdx>s) pathIdx=s; // Protect the path index
// Ramp the command up
speedCmd = RateLimit(speedCmd, START_RATE,speedCmdZ1);
leftmotors=Pid1(speedCmd+dirCmd);
rightmotors=Pid2(speedCmd-dirCmd);
//rightmotors=speedCmd-dirCmd;
//leftmotors=speedCmd+dirCmd;
motor[rtBack]=rightmotors;
motor[rtMotor]=rightmotors;
motor[ltMotor]=leftmotors;
motor[ltBack]=leftmotors;
motor[blockthrower]=armSpd;
//DebugInt("rightmotors",rightmotors);
//DebugInt("leftmotors",leftmotors);
//DebugInt("rightencoder",nMotorEncoder[rtMotor]);
//DebugInt("leftencoder",nMotorEncoder[ltMotor]);
if (iFrameCnt==0)
writeDebugStreamLine("i,pthIdx,rbtDist,irval,spdCmd,IR,state, rightencoder, leftencoder");
writeDebugStreamLine("%3i,%3i,%4i,%4i,%3i,%3i,%3i, %4i, %4i",iFrameCnt,pathIdx,distInches,IRval,speedCmd,SensorValue[IR],state, nMotorEncoder[rtMotor], nMotorEncoder[ltMotor]);
//--------------------------Robot Code--------------------------//
// Wait for next itteration
iFrameCnt++;
timeLeft=FOREGROUND_MS-time1[T1];
releaseCPU();
wait1Msec(timeLeft);
}// While
}//Foreground
void dumpDebugInfo()
{
writeDebugStreamLine("VEX Master Firmware version: %d", version);
writeDebugStreamLine("Firmware version: %d", nVexMasterVersion);
writeDebugStreamLine("Main battery level: %3.2f", nAvgBatteryLevel * 0.001);
writeDebugStreamLine("Backup battery level: %3.2f", BackupBatteryLevel * 0.001);
writeDebugStreamLine("Is autonomous mode: %s", bIfiAutonomousMode ? "True" : "False");
writeDebugStreamLine("Transmitter state:");
TVexReceiverState state = nVexRCReceiveState;
if (state & vrNoXmiters)
writeDebugStreamLine("\tTransmitter powered off");
if (state & vrXmit1)
writeDebugStreamLine("\tTransmitter 1 on");
if (state & vrXmit2)
writeDebugStreamLine("\tTransmitter 2 on");
if (state & vrCompetitionSwitch)
writeDebugStreamLine("\tCompetition switch attached");
if (state & vrGameController)
writeDebugStreamLine("\tVEXnet Controller");
else
writeDebugStreamLine("\tLegacy 75MHz controller");
if (state & vrAutonomousMode)
writeDebugStreamLine("\tAutonomous mode");
else
writeDebugStreamLine("\tDriver control mode");
if (state & vrDisabled)
writeDebugStreamLine("\tDisabled");
else
writeDebugStreamLine("\tEnabled");
int upTime = nSysTime;
int hr = upTime / 1000 / 60 / 60;
if (hr > 0) upTime -= hr * 60 * 60 * 1000;
int min = upTime / 1000 / 60;
if (min > 0) upTime -= min * 60 * 1000;
int sec = upTime / 1000;
if (sec > 0) upTime -= sec * 1000;
int ms = upTime;
writeDebugStreamLine("System up time: %02d:%02d:%02d.%04d", hr, min, sec, ms);
int pgmTime = nPgmTime;
int pHr = pgmTime / 1000 / 60 / 60;
if (pHr > 0) pgmTime -= pHr * 60 * 60 * 1000;
int pMin = pgmTime / 1000 / 60;
if (pMin > 0) pgmTime -= pMin * 60 * 1000;
int pSec = pgmTime / 1000;
if (pSec > 0) pgmTime -= pSec * 1000;
int pMs = pgmTime;
writeDebugStreamLine("Program run time: %02d:%02d:%02d.%04d", pHr, pMin, pSec, pMs);
}
