task main()
{
forward(1000); //change the value of 1000 to change how long the
backward(1000); //robot moves forward, backward, or turns
rightTurn(1000);
leftTurn(1000);
}
void gyroTurn(float power, float fDegrees, eDirection direct)
{//Robot turns at a specific power, a certain angle, either right or left
HTGYROstartCal(gyro); //Activate gyroscope
wait1Msec(100); //Let it adjust
float fCurrent = 0.0; //Set heading to 0 degrees
float fRotSpeed = 0.0; //Current rotational speed is 0
if (direct == dLeft) //If told to turn left
{
leftTurn(power); //Set motors to turn left
}
else //If tol to turn right
{
rightTurn(power); //Set motors to turn right
}
do //Loop through following until conditions are met
{
wait1Msec(MEASUREMENT_MS); //Give a moment to recalibrate
fRotSpeed = HTGYROreadRot(gyro); //Rotation speed is now the gyro's input
fCurrent += fRotSpeed * (MEASUREMENT_MS / 1000.0); //Current heading is what is was before
//plus the value of the rate of turn times the amount of time that rate value was taken
} while (abs(fCurrent) < fDegrees); //Repeat until the heading is the amount told to turn
stopMotors(); //You got there, now stop
}
Polygon upperHull(const Point &left, const Point &right, Polygon &between) {
between.push_back(right);
Polygon out;
out.push_back(left);
Point lastPeek = right;
for(Polygon::const_iterator it = between.begin(); it != between.end(); ++it) {
if(it->xx == lastPeek.xx) {
if(it->yy < lastPeek.yy) {
continue;
}
else {
lastPeek = out.back();
out.pop_back();
}
}
while(!out.empty() && !rightTurn(out.back(), lastPeek, *it)) {
lastPeek = out.back();
out.pop_back();
}
out.push_back(lastPeek);
lastPeek = *it;
}
// Fix rightmost point as it should be handled differently:
out.push_back(lastPeek);
if(out.back() == right) {
out.pop_back();
}
return out;
}
task main()
{
wait1Msec(2000); // Robot waits for 2000 milliseconds before executing program
rightTurn();
fowardTime (3000);
leftTurn();
fowardTime(5000);
leftTurn();
fowardTime(6000);
rightTurn();
fowardTime(4000);
rightTurn ();
fowardTime(6000);
leftTurn ();
fowardTime (4000);
leftTurn ();
fowardTime (6000);
rightTurn();
fowardTime (4000);
} // Program ends, and the robot stops
task main()
{
while(true)
{
if(VexRT(Btn5U)) {
leftTurn(1);
wait10Msec(100);
}
if(VexRT(Btn6U)) {
rightTurn(1);
wait10Msec(100);
}
if(VexRT(Btn8U)) {
move(2);
wait10Msec(100);
}
if(VexRT(Btn8D)) {
move(-2);
wait10Msec(100);
}
if(VexRT(Btn7U)) {
rightTurn(360);
}
if(vexRT(Btn8R)) {
rightTurn(2);
move(20);
leftTurn(10);
move(30);
intake(true);
fly(3);
wait10Msec(1000);
rest();
}
}
}
void faceIR(tSensors ir_seeker)
{
int ir_value = getIRReading(/*ir_seeker*/);
do
{
if(ir_value < 2)
{
leftTurn(50);
}
else if(ir_value > 2)
{
rightTurn(50);
}
wait1Msec(1);
ir_value = getIRReading(/*ir_seeker*/);
}
while(ir_value != 2);
}
__interrupt void interrupt2() {
if (P2IFG & I2_RIGHT_BUTTON) {
switch (turnState) {
case NO_TURN:
rightTurn();
autoCancelState = AUTO_CANCEL_DISABLED;
break;
case LEFT_TURN:
cancelTurn();
break;
case RIGHT_TURN:
cancelTurn();
break;
}
P2IFG &= ~I2_RIGHT_BUTTON; // Clear interrupt flag
}
LPM4_EXIT;
}
void
GLHelper::drawBoxLines(const PositionVector& geom,
const std::vector<SUMOReal>& rots,
const std::vector<SUMOReal>& lengths,
SUMOReal width, int cornerDetail, SUMOReal offset) {
// draw the lane
int e = (int) geom.size() - 1;
for (int i = 0; i < e; i++) {
drawBoxLine(geom[i], rots[i], lengths[i], width, offset);
}
// draw the corner details
if (cornerDetail > 0) {
for (int i = 1; i < e; i++) {
glPushMatrix();
glTranslated(geom[i].x(), geom[i].y(), 0.1);
if (rightTurn(rots[i - 1], rots[i])) {
// inside corner
drawFilledCircle(MIN2(lengths[i], width - offset), cornerDetail);
} else {
// outside corner, make sure to only draw a segment of the circle
SUMOReal angleBeg = -rots[i - 1];
SUMOReal angleEnd = 180 - rots[i];
// avoid drawing more than 360 degrees
if (angleEnd - angleBeg > 360) {
angleBeg += 360;
}
if (angleEnd - angleBeg < -360) {
angleEnd += 360;
}
// for a left tur, draw the right way around
if (angleEnd > angleBeg) {
angleEnd -= 360;
}
drawFilledCircle(MIN2(lengths[i], width + offset), cornerDetail, angleBeg, angleEnd);
}
glEnd();
glPopMatrix();
}
}
}
void
GLHelper::drawBoxLines(const PositionVector& geom,
const std::vector<double>& rots,
const std::vector<double>& lengths,
double width, int cornerDetail, double offset) {
// draw the lane
int e = (int) geom.size() - 1;
for (int i = 0; i < e; i++) {
drawBoxLine(geom[i], rots[i], lengths[i], width, offset);
}
// draw the corner details
if (cornerDetail > 0) {
for (int i = 1; i < e; i++) {
glPushMatrix();
glTranslated(geom[i].x(), geom[i].y(), 0.1);
double angleBeg = -rots[i - 1];
double angleEnd = 180 - rots[i];
if (rightTurn(rots[i - 1], rots[i])) {
std::swap(angleBeg, angleEnd);
}
// only draw the missing piece
angleBeg -= 90;
angleEnd += 90;
// avoid drawing more than 360 degrees
if (angleEnd - angleBeg > 360) {
angleBeg += 360;
}
if (angleEnd - angleBeg < -360) {
angleEnd += 360;
}
// draw the right way around
if (angleEnd > angleBeg) {
angleEnd -= 360;
}
drawFilledCircle(width + offset, cornerDetail, angleBeg, angleEnd);
glPopMatrix();
}
}
}
task FigureEight() //Starts first task, task main()
{
while(1==1)
{
for(int i = 0; i <= 3; i++) // initialize int 'i' to 0, and run the loop as long as 'i' is less than 3,
// incrementing 'i' by 1 after each iteration of the loop
{
moveForward(15);
rightTurn();
}
for(int i = 0; i <= 3; i++) // initialize int 'i' to 0, and run the loop as long as 'i' is less than 3,
// incrementing 'i' by 1 after each iteration of the loop
{
moveForward(15);
leftTurn();
}
}
}
void
play(void)
{
MWEvent event;
MW244BPacket incoming;
event.eventDetail = &incoming;
while (TRUE) {
NextEvent(&event, M->theSocket());
if (!M->peeking())
switch(event.eventType) {
case EVENT_A:
aboutFace();
break;
case EVENT_S:
leftTurn();
break;
case EVENT_D:
forward();
break;
case EVENT_F:
rightTurn();
break;
case EVENT_BAR:
backward();
break;
case EVENT_LEFT_D:
peekLeft();
break;
case EVENT_MIDDLE_D:
shoot();
break;
case EVENT_RIGHT_D:
peekRight();
break;
case EVENT_NETWORK:
processPacket(&event);
break;
case EVENT_INT:
quit(0);
break;
}
else
switch (event.eventType) {
case EVENT_RIGHT_U:
case EVENT_LEFT_U:
peekStop();
break;
case EVENT_NETWORK:
processPacket(&event);
break;
}
ratStates(); /* clean house */
manageMissiles();
DoViewUpdate();
mws_update(M->state);
/* Any info to send over network? */
}
}
int main(void)
{
//Decs
int right_sensor= 0;
int center_sensor = 1;
int left_sensor = 2;
int dark = 500;
int light_dark = 500;
int last_turn = 0;
printf("Starting line following program\n");
printf("Center the robot and calibrate the left then right sensor to begin\n\n");
//While the B button is not pressed, wait for input
while(analog10(left_sensor) < 600)
{
}
printf("Left sensor is ready\n\n");
while(analog10(right_sensor) < 600)
{
}
printf("Right sensor is ready\n\n");
printf("Press B to begin\n");
while(b_button() == 0)
{
}
printf("Running\n\n");
move(25, 100);
while(1==1)
{
//While there is not turn available, keep on keeping on.
while(isTurnAvailable() == 0)
{
moveForward(last_turn);
}
printf("Turn is available\n");
if(isTurnAvailable() == 2)
{
leftTurn();
last_turn = 1;
}
if(isTurnAvailable() == 1)
{
rightTurn();
last_turn = 0;
}
}//End of while loop
printf("Done");
return 0;
}
/* ----------------------------------------------------------------------- */
void play(void)
{ // initialisation
cout << "Starting Play" << endl;
//SetRatPosition(7,1,5,0);
proj.present = false;
proj.prev_x = proj.prev_y = proj.x = proj.y = 1;
int turn = 0;
updateSeqNo = true;
join = false;
for (int k = 0; k < 8; k++)
participants[k] = prevseq_a[k] = 0;
for (int k = 0; k < 8; k++)
expected_seqno[k] = k+1;
GLOBAL_ID = 0;
checking = 1;
checkingzero=1;
MWEvent event;
MW244BPacket incoming;
event.eventDetail = &incoming;
while (TRUE) {
turn ++;
NextEvent(&event, M->theSocket());
if (!M->peeking())
switch(event.eventType) {
case EVENT_A:
aboutFace();
break;
case EVENT_S:
leftTurn();
break;
case EVENT_D:
forward();
break;
case EVENT_F:
rightTurn();
break;
case EVENT_LEFT_D:
peekLeft();
break;
case EVENT_BAR:
shoot();
break;
case EVENT_RIGHT_D:
peekRight();
break;
case EVENT_NETWORK:
processPacket(&event);
break;
case EVENT_TIMEOUT:
checking = (++checking) % 25;
if (checking == 0 && !join){
cout << "Setting ID to zero" << endl;
join = true;
setMapping();
}
else {cout << "TIMEOUT" << endl;
participation = (++participation) % 30;
manageMissiles();
if (participation == 29)
{ ratStates(); /* clean house */
for (int i = 0 ; i < 8 ; i++)
participants[i] = 0;
}
}
break;
case EVENT_INT:
quit(0);
break;
}
else
switch (event.eventType) {
case EVENT_RIGHT_U:
case EVENT_LEFT_U:
peekStop();
break;
case EVENT_NETWORK:
processPacket(&event);
break;
}
DoViewUpdate();
checking ++;
manageMissiles();
if (join && checkingzero == 0)
{
MW244BPacket p;
makePacket(&p,'a',kills, updateSeqNo);
sendPacketToPlayers(p);
}
else if (join && turn % 2 == 0)
{
MW244BPacket p;
makePacket(&p,'t',-1, updateSeqNo);
sendPacketToPlayers(p);
}
}
}
void performLogic(uint8_t buffer [] , uint8_t length, MoveTaskStruct *moveTPtr, SensorAStruct *sensorT)
{
webServerTaskStruct *webServerData = sensorT->webServerData;
uint16_t frontDistanceInches = getFrontSensorDistanceInInches(buffer, length);
uint16_t topRightDistanceInches = getTopRightSensorDistanceInInches(buffer, length);
uint16_t bottomRightDistanceInches = getBottomRightSensorDistanceInInches(buffer, length);
statePrintedToWebServer = current_state;
if (current_state == GLOBAL_READ) {
printf("GLOBAL READ \n");
}
else if (current_state == GETTING_IN_3_FEET){
printf("Get In 3 Feet \n");
}
else if (current_state == AVOID_OBSTACLE_1){
printf("AVOID_OBSTACLE_1 \n");
}
else
printf("Invalid State \n");
switch (current_state) {
case GLOBAL_READ:
{
//Check whether both sensor values out of threshold
uint8_t outOfRange = sideSensorOutOfRange(topRightDistanceInches, bottomRightDistanceInches);
//printf("OutOfRange %d\n" , outOfRange);
if (outOfRange == 1) {
// Turn right and go to the state where u wait for the side
// sensor values to be in range or the front is not clear
if (count == 0) {
moveForward(moveTPtr, sensorT->mapT,webServerData, 8);
count++;
return;
}
if (prev_state == AVOID_OBSTACLE_1) {
prev_state = GLOBAL_READ;
current_state = GETTING_IN_3_FEET;
}
else {
prev_state = GLOBAL_READ;
current_state = AVOID_OBSTACLE_1;
}
count = 0;
rightTurn(moveTPtr, sensorT->mapT,webServerData);
return;
}
//Check whether both sensor values are highly deviated
uint8_t inHighTolerance = sideSensorWithinHighTolerance(topRightDistanceInches, bottomRightDistanceInches);
// | |-----------
// | |
// | |----
//Rover not in high tolerance (posibly crossing the obstacle
if (inHighTolerance == 0) {
// Move a little forward and go to the state where u wait for the side
// sensor values to be in range or the front is not clear
uint8_t isFrontClear = frontSensorClear(frontDistanceInches);
if (isFrontClear == 1)
{
//printf("Move Forward\n");
moveForward(moveTPtr, sensorT->mapT,webServerData, 12);
return;
}
else if (getOneFeetToWall(moveTPtr, webServerData, sensorT->mapT, frontDistanceInches))
{
return;
}
else {
//printf("Left Turn\n");
leftTurn(moveTPtr, sensorT->mapT,webServerData);
}
return;
}
// Check Whether the sensor values are low deviated
uint8_t isInTolerance= sideSensorsWithinTolerance(topRightDistanceInches,
bottomRightDistanceInches);
if (isInTolerance == 0 && tiltCount <= 3)
{
if (topRightDistanceInches > bottomRightDistanceInches)
tiltRover(moveTPtr, sensorT->mapT,webServerData, 10, right_turn_ToRover);
else
tiltRover(moveTPtr, sensorT->mapT,webServerData, 10, left_turn_ToRover);
return;
}
tiltCount = 0;
// Then check if the rover is within 3 feet
uint8_t in3Feet = roverWithin3Feet(topRightDistanceInches, bottomRightDistanceInches);
if (in3Feet == 0) {
rightTurn(moveTPtr, sensorT->mapT,webServerData);
prev_state = GLOBAL_READ;
current_state = GETTING_IN_3_FEET;
return;
}
// if (topRightDistanceInches <= 9 && bottomRightDistanceInches <= 9){
// leftTurn(moveTPtr, sensorT->mapT,webServerData);
// return;
// }
uint8_t isFrontClear = frontSensorClear(frontDistanceInches);
if (isFrontClear == 1)
{
//printf("Move Forward\n");
if ( RUN == 2 )
{
if ( frontDistanceInches > clearDistance(sensorT->mapT, 0) && isDistanceFromMapValid == 1)
{
moveForward(moveTPtr, sensorT->mapT,webServerData, clearDistance(sensorT->mapT, 0));
}
else
{
moveForwardOneUnit(moveTPtr, sensorT->mapT,webServerData);
isDistanceFromMapValid = 0;
}
}
else
{
moveForwardOneUnit(moveTPtr, sensorT->mapT,webServerData);
}
}
else if (getOneFeetToWall(moveTPtr, webServerData, sensorT->mapT, frontDistanceInches))
{
return;
}
else {
//printf("Left Turn\n");
leftTurn(moveTPtr, sensorT->mapT,webServerData);
}
return;
};
case GETTING_IN_3_FEET:
{
uint8_t inRange = roverWithin3Feet(topRightDistanceInches, topRightDistanceInches);
if (inRange == 1) {
prev_state = GETTING_IN_3_FEET;
current_state = GLOBAL_READ;
break;
}
uint8_t isFrontClear = frontSensorClear(frontDistanceInches);
if (isFrontClear == 1)
{
//printf("Move Forward\n");
if ( RUN == 2 )
{
if ( frontDistanceInches > clearDistance(sensorT->mapT, 0) && isDistanceFromMapValid == 1)
{
moveForward(moveTPtr, sensorT->mapT,webServerData, clearDistance(sensorT->mapT, 0));
}
else
{
moveForwardOneUnit(moveTPtr, sensorT->mapT,webServerData);
isDistanceFromMapValid = 0;
}
}
else
{
moveForwardOneUnit(moveTPtr, sensorT->mapT,webServerData);
}
break;
}
else if (getOneFeetToWall(moveTPtr, webServerData, sensorT->mapT, frontDistanceInches))
{
break;
}
else {
prev_state = GETTING_IN_3_FEET;
current_state = GLOBAL_READ;
leftTurn(moveTPtr, sensorT->mapT,webServerData);
break;
}
break;
};
case AVOID_OBSTACLE_1:
{
// Check Whether the sensor values are low deviated
uint8_t inRange = roverWithin3Feet(topRightDistanceInches, topRightDistanceInches);
if (inRange == 1) {
prev_state = AVOID_OBSTACLE_1;
current_state = GLOBAL_READ;
break;
}
uint8_t isFrontClear = frontSensorClear(frontDistanceInches);
if (isFrontClear == 1)
{
moveForwardOneUnit(moveTPtr, sensorT->mapT,webServerData);
}
else if (getOneFeetToWall(moveTPtr, webServerData, sensorT->mapT, frontDistanceInches))
{
break;
}
else {
//printf("Left Turn\n");
prev_state = AVOID_OBSTACLE_1;
current_state = GLOBAL_READ;
leftTurn(moveTPtr, sensorT->mapT,webServerData);
}
break;
};
default:
break;
}
}
