int main(){
connect_to_robot();
initialize_robot();
setAngles();
firsteight();
}
int main ()
{
connect_to_robot();
initialize_robot();
setAnglesStart();
//wallFollowingR();
wallFollowingL();
}
int main(){
connect_to_robot();
initialize_robot();
set_origin();
double curr_coord[2] = {0, 0};
spin(curr_coord, to_rad(90));
sleep(1);
spin(curr_coord, to_rad(180));
return 0;
}
void setup()
{
connect_to_robot();
initialize_robot();
set_ir_angle(LEFT, -45);
set_ir_angle(RIGHT, 45);
set_origin();
set_point(0, 0);
print_square_centres();
get_motor_encoders(&leftprev, &rightprev);
moveBackwards(PHASE1_SPEED, PHASE1_SLOW, START_OFFSET * CM_TO_ENCODER); //move robot to centre of first square.
}
task main()
{
int i;
int center_position;
initialize_robot();
//waitForStart();
// startTask(raise_elbow);
startTask(raise_hand);
startTask(lower_ramp);
// initialize_receiver(irr_left, irr_right);
servo[leftEye] = LSERVO_CENTER;
servo[rightEye] = RSERVO_CENTER;
center_position = ultrasound_mk(carrot, -24, US_DIST_POS_1, US_DIST_POS_2, US_DIST_POS_3);
//startTask(raise_ramp);
if (center_position == -1) {
playImmediateTone(251, 150);
}
for (i = 0; i < center_position; i++) {
playImmediateTone(251, 50);
wait1Msec(1000);
}
nMotorEncoder[elbow] = 0;
motor[elbow] = 20;
while (nMotorEncoder[elbow] <= 3600) {
nxtDisplayCenteredBigTextLine(3, "%d", nMotorEncoder[elbow]);
}
motor[elbow] = 0;
// move_to_pole(center_position);
while(true) {
nxtDisplayCenteredBigTextLine(3, "%d", USreadDist(carrot));
}
}
int main()
{
connect_to_robot();
initialize_robot();
setAngles();
inputMotors();
//wallFollowing();
int i;
for (i=0; i<4; i++){
printf("%d\n", i);
thetaAcc = 0.0;
SPEED = SPEED + 8;
//wallFollowing();
wallFollowingR();
}
}
int main(){
connect_to_robot();
initialize_robot();
set_origin();
set_ir_angle(LEFT, -45);
set_ir_angle(RIGHT, 45);
initialize_maze();
reset_motor_encoders();
int i;
for (i = 0; i < 17; i++){
set_point(nodes[i]->x, nodes[i]->y);
}
double curr_coord[2] = {0, 0};
map(curr_coord, nodes[0]);
breadthFirstSearch(nodes[0]);
reversePath(nodes[16]);
printPath(nodes[0]);
struct point* tail = malloc(sizeof(struct point));
tail->x = nodes[0]->x;
tail->y = nodes[0]->y;
struct point* startpoint = tail;
build_path(tail, nodes[0]);
// Traverse to end node.
while(tail->next){
set_point(tail->x, tail->y); // Visual display for Simulator only.
tail = tail->next;
}
tail->next = NULL; // Final node point to null.
printf("tail: X = %f Y = %f \n", tail->x, tail->y);
parallel(curr_coord);
spin(curr_coord, to_rad(180));
sleep(2);
set_ir_angle(LEFT, 45);
set_ir_angle(RIGHT, -45);
mazeRace(curr_coord, nodes[0]);
return 0;
}
int main() {
connect_to_robot();
initialize_robot();
set_origin();
printf("Ghandy-Bot activated, destroy!!!\n");
//Set front IR sensors to to face left and right
set_ir_angle(LEFT, -45);
set_ir_angle(RIGHT, 45);
init_map();
print_map();
robot.map[1][1].walls[0] = 1;
robot.map[1][2].walls[2] = 1;
robot.map[1][1].walls[1] = 1;
robot.map[2][1].walls[3] = 1;
robot.map[1][3].walls[2] = 1;
robot.map[1][2].walls[0] = 1;
robot.map[0][4].walls[2] = 1;
robot.map[0][3].walls[0] = 1;
robot.map[3][4].walls[3] = 1;
robot.map[2][4].walls[1] = 1;
robot.map[3][2].walls[2] = 1;
robot.map[3][1].walls[0] = 1;
robot.map[3][2].walls[3] = 1;
robot.map[2][2].walls[1] = 1;
int i;
for(i=1;i<=4;i++)
robot.map[0][i].walls[3] = 1;
for(i=1;i<=4;i++)
robot.map[i][1].walls[2] = 1;
for(i=1;i<=4;i++)
robot.map[3][i].walls[1] = 1;
print_map();
/* Phase 1 */
//Perform depth first search on maze
//dfs(0, 0, M_PI / 2);
/* Phase 2 */
lee(0,1); //Update matrix with Lee costs from node [0,1]
/* Generate shortest path to node [3,4] (finish line) */
generate_path(3,4);
follow_path(); //Follow the generated path to finish
//Phew, did we win???
printf("Ghandy-Bot deactivated!\n");
return 0;
}
void setup()
{
// Add your initialization code here
uint8_t count = 10;
// join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();
// initialize serial communication
// (115200 chosen because it is required for Teapot Demo output, but it's
// really up to you depending on your project)
Serial.begin(115200);
// while (!Serial); // wait for Leonardo enumeration, others continue immediately
// // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
// // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
// // the baud timing being too misaligned with processor ticks. You must use
// // 38400 or slower in these cases, or use some kind of external separate
// // crystal solution for the UART timer.
// pinMode(IRQ_PIN, INPUT);
// digitalWrite(IRQ_PIN, HIGH);
// // initialize device
// Serial.println(F("Initializing I2C devices..."));
// mpu.initialize();
// // verify connection
// Serial.println(F("Testing device connections..."));
// Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// /*
// // wait for ready
// Serial.println(F("\nSend any character to begin DMP programming and demo: "));
// while (Serial.available() && Serial.read()); // empty buffer
// while (!Serial.available()); // wait for data
// while (Serial.available() && Serial.read()); // empty buffer again
// */
// // load and configure the DMP
// Serial.println(F("Initializing DMP..."));
// do {
// devStatus = mpu.dmpInitialize();
// // make sure it worked (returns 0 if so)
// if (devStatus == 0) {
// count = 10;
// // turn on the DMP, now that it's ready
// Serial.println(F("Enabling DMP..."));
// mpu.setDMPEnabled(true);
// // enable Arduino interrupt detection
// Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
// attachInterrupt(DMP_IRQ, dmpDataReady, RISING);
// mpuIntStatus = mpu.getIntStatus();
// // set our DMP Ready flag so the main loop() function knows it's okay to use it
// Serial.println(F("DMP ready! Waiting for first interrupt..."));
// dmpReady = true;
// // get expected DMP packet size for later comparison
// packetSize = mpu.dmpGetFIFOPacketSize();
// // exit the loop since everything is configured
// // configure LED for output
// pinMode(LED_PIN, OUTPUT);
// return;
// } else {
// // ERROR!
// // 1 = initial memory load failed
// // 2 = DMP configuration updates failed
// // (if it's going to break, usually the code will be 1)
// Serial.print(F("DMP Initialization failed (code "));
// Serial.print(devStatus);
// Serial.println(F(")"));
// // New attempt message
// Serial.println("Trying again");
// }
// }
// while (--count);
// // configure LED for output
// pinMode(LED_PIN, OUTPUT);
// // Check if the configuration has failed
// // if (!count) {
// Serial.println("DMP initialization failed");
// while (true) {
// // Locks in infinite loop
// digitalWrite(LED_PIN, 0);
// delay(300);
// digitalWrite(LED_PIN, 1);
// delay(300);
// }
// // }
while (!Serial)
{
digitalWrite(13, HIGH);
delay(300);
digitalWrite(13, LOW);
delay(300);
}
pinMode (IRQ_PORT, OUTPUT);
digitalWrite(IRQ_PORT, LOW);
initialize_robot();
isConnected = true;
Serial.print(PROMPT);
}
