/*
* main.c
* Description: Makes the robot move in the corresponding directions.
* The length of delay cycles after each function call determines how
* long/far the robot moves in that particular direction.
*/
int main(void) {
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
InitPinsOut();
InitTimer();
while (1) {
//move forward
moveForward();
_delay_cycles(1000000);
//move backward
moveBackward();
_delay_cycles(1000000);
//complete a small (<45 degree) right turn
rightTurn();
_delay_cycles(500000);
//complete a small (<45 degree) left turn
leftTurn();
_delay_cycles(500000);
//complete a large (>45 degree) right turn
rightTurn();
_delay_cycles(1000000);
//complete a large (>45 degree) left turn
leftTurn();
_delay_cycles(1000000);
}
return 0;
}
int main(void) {
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
initPinOuts();
timersConfig();
while (1) {
leftTurn();
__delay_cycles(500000);
rightTurn();
__delay_cycles(500000);
moveForward();
__delay_cycles(2500000);
moveBackward();
__delay_cycles(2500000);
leftTurn();
__delay_cycles(1000000);
rightTurn();
__delay_cycles(1000000);
}
return 0;
}
void main (void)
{
initialize();
wait1Msec(200);
//initialize values
int pwr = 15;
int sensorState = -1;
//determine these values by running the calibration program first
int threshold1 = 150;
int threshold2 = 150;
int delay = 10;
//initialize motors and LED indicators
setMotor (MOTOR_A, 0);
setMotor (MOTOR_B, 0);
LEDnum(0);
LEDoff(RED_LED);
LEDoff(GREEN_LED);
//waits for a button press
while(getSensor(BUTTON) == 0);
wait1Msec(100);
while(getSensor(BUTTON) == 1);
resetTimer();
while(getSensor(BUTTON)== 0 && time100() < 64) {
//line follows for 6.4 seconds
if (sensorState != getSensorState(threshold1, threshold2)) {
//only change direction if conditions are different than before.
sensorState = getSensorState(threshold1,threshold2);
if (sensorState == 0) { //go straight
drive(15, delay);
} else if (sensorState == 1){
rightTurn(15,delay);
} else if (sensorState == 2){
leftTurn(15, delay);
} else if (sensorState == 3){
rightTurn(15, delay);
}
}
}
//hard code exit 2
rightTurn(pwr,2400);
drive(pwr,9400);
leftTurn(pwr,1500);
drive(pwr,20000);
}
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
}
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);
}
__interrupt void interrupt1() {
if (P1IFG & I1_LEFT_BUTTON) {
switch (turnState) {
case NO_TURN:
leftTurn();
autoCancelState = AUTO_CANCEL_DISABLED;
break;
case LEFT_TURN:
cancelTurn();
break;
case RIGHT_TURN:
cancelTurn();
break;
}
P1IFG &= ~I1_LEFT_BUTTON; // Clear interrupt flag
}
if (P1IFG & I1_LEFT_BRAKE || P1IFG & I1_RIGHT_BRAKE) {
switch (brakeState) {
case NO_BRAKES:
setBrakes();
break;
}
P1IFG &= ~(I1_LEFT_BRAKE + I1_RIGHT_BRAKE); // Clear interrupt flag
}
LPM4_EXIT;
}
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
int main(int argc, char *argv[])
{
// set_conio_terminal_mode();
fdFifo = open("./RobotFifo", O_WRONLY);
for (int n = 0; n < 4; n++)
{
forward(40, 5);
leftTurn(4);
}
stop();
}
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();
}
}
}
task main()
{
initializeRobot();
waitForStart();
raiseArm(ArmUpRotations);
intializeClaw();
int _dirAC = 0;
wait1Msec(200);
int sensorDirection = HTIRS2readDCDir(irSeeker);
if(sensorDirection < 5){
raiseArm(LeftArmUpRotations);
leftTurn(leftTurnRotations);
attackRack(leftFinalInches);
}
else{
firstMove(middleStartInches);
wait1Msec(300);
sensorDirection = HTIRS2readDCDir(irSeeker);
if(sensorDirection < 7){
leftTurn(middleTurnRotations);
attackRack(middleFinalInches);
}
else{
raiseArm(RightArmUpRotations);
firstMove(rightStartInches - middleStartInches);
leftTurn(middleTurnRotations);
attackRack(rightFinalInches);
}
}
retreat();
}
ConvexHull(const Polygon &scatteredPoints) {
const int numPoints = scatteredPoints.size();
Polygon copy(scatteredPoints);
// Sort input by x (lex):
std::sort(©[0], ©[numPoints]);
// Find left and rightmost points:
Point leftMost = copy[0];
Point rightMost = copy[numPoints-1];
//Split points in upper and lower:
Polygon upperPoints;
Polygon lowerPoints;
for(int i = 1; i < numPoints-1; ++i) {
Point p = copy[i];
if(colinear(leftMost, rightMost, p)) {
continue;
}
if(leftTurn(leftMost, rightMost, p)) {
upperPoints.push_back(p);
}
else {
lowerPoints.push_back(flipY(p));
}
}
upperPoints = upperHull(leftMost, rightMost, upperPoints);
lowerPoints = upperHull(flipY(leftMost), flipY(rightMost), lowerPoints);
//Make points of hull:
int actualNumPoints = size = upperPoints.size() + lowerPoints.size();
points = new Point[actualNumPoints];
int i = 0;
for(Polygon::const_iterator it = upperPoints.begin(); it != upperPoints.end(); ++it) {
points[i++] = *it;
}
points[i] = rightMost;
i = actualNumPoints;
for(Polygon::const_iterator it = lowerPoints.begin(); it != lowerPoints.end(); ++it) {
i--;
if(i != actualNumPoints-1)
points[i+1] = flipY(*it);
}
std::cout << actualNumPoints;
for(i = 0; i < actualNumPoints; ++i)
std::cout << " " << points[i].first << " " << points[i].second;
std::cout << std::endl;
}
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);
}
//Realiza el algoritmo de Graham de las 3 monedas para hallar el Convex Hull S de una lista de Puntos Q.
//Devuelve una Pila con el resultado clockwise.
void grahamScan(std::list<Point2D> & Q, std::stack<Point2D> & S){
minimal = encontrarMinimal(Q); //Encuentra el minimal izquierda abajo
// std::cout<<"Minimal: "; minimal.print();
borrarMinimal(Q, minimal); //Borra el minimal de la cola
Q.sort(comparePoint2DPolar); //Ordena en forma polar
std::cout<<"Lista ordenada\n"; printList(Q);
eliminarColineales(Q); //Hace limpieza de los puntos colineales, dejando el mas lejano
std::cout<<"Lista ordenada\n"; printList(Q);
//Ubica las 3 primeras monedas
S.push(minimal); //Agrega el primero que es el minimal
//Agrega la segunda y tercera
std::list<Point2D>::iterator it = Q.begin(); //Iterador para recorrer la Q
for(unsigned int i = 0; i < 2 and it != Q.end(); i++, it++){
S.push(*it);
}
//tamanio de Q
unsigned int n = Q.size();
//Loop de Graham Scan
for(unsigned int i = 2; i < n and it != Q.end(); i++, it++){
Point2D ntt = nextToTop(S);
Point2D nt = top(S);
Point2D p = *it;
while(!leftTurn(ntt, nt, p) and (S.size() > 1)){ //Si no froman un giro a la izquierda y queda mas de un elemento en S
// printStack(S);
S.pop(); //Saco el tope de S
ntt = nextToTop(S); //Renuevo los valores y vuelvo a probar
nt = top(S);
}
S.push(p); //Agrego el elemento a S
}
}
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();
}
}
}
bool Util::leftTurn(const QVector2D& a, const QVector2D& b, const QVector2D& c) {
return leftTurn(b - a, c - b);
}
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? */
}
}
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;
}
}
/* ----------------------------------------------------------------------- */
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);
}
}
}
// Point inside triangle
// Returns true iff point (xx,yy) is inside the counter-clockwise triangle (x[3],y[3])
// REQUIRES: leftTurn()
bool pointInsideTriangle( double x[], double y[], double xx, double yy ){
return leftTurn(x[0], y[0], x[1], y[1], xx, yy)
&& leftTurn(x[1], y[1], x[2], y[2], xx, yy)
&& leftTurn(x[2], y[2], x[0], y[0], xx, yy);
}
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;
}
