// This is the main loop TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart) { // -------- Start Switch/Button------- // Switch/Button - see switch.h // To test if it is pressed then if(SWITCH_pressed(&button)){ // pressed } // To test if it is released then if(SWITCH_released(&button)){ // released } // -------- End Switch/Button------- // -------- Start Marquee------- // Marquee - see 'segled.h' // Before using the Marquee you need to redirect rprintf to write to it // This can be done using Writer old = rprintfInit(marqueeGetWriter(&marquee)); // All rprintf output will then be sent to the marquee but will not // display until an end-of-line eg "\n" has been sent. Example:- // rprintf("Hello World\n"); // If the endDelay is non-zero then the marquee will scroll // forever or until you call: marqueeStop(&marquee); // If the endDelay is zero then the marquee will stop once // the entire line has been shown ('one-shot' mode) // In 'one-shot' mode then you may want to make sure that // a previous line has finished before you display a second line. // This can be done as follows:- marqueeSetEndDelay(&marquee,0); // Make sure we are in one-shot mode if(marqueeIsActive(&marquee)==FALSE){ if(loopCount==1){ rprintf("ABCDEFGHIJKLMNOPQRSTUVWXYZ\n"); }else{ rprintf("Loop=%u\n",(unsigned)loopCount); // Put the loop count } } // Restore rprintf back to its previous location rprintfInit(old); // -------- End Marquee------- // -------- Start Sharp GP2------- // Read the Sharp GP2 and store the result in ir_0.distance.cm distanceRead(ir_0); // The value can be printed using %u eg rprintf("Distance=%u",ir_0.distance.cm); // or dumped using: rprintf("ir_0: "); distanceDump(ir_0); rprintfCRLF(); // -------- End Sharp GP2------- // -------- Start Sharp GP2------- // Read the Sharp GP2 and store the result in ir_1.distance.cm distanceRead(ir_1); // The value can be printed using %u eg rprintf("Distance=%u",ir_1.distance.cm); // or dumped using: rprintf("ir_1: "); distanceDump(ir_1); rprintfCRLF(); // -------- End Sharp GP2------- // -------- Start Sharp GP2------- // Read the Sharp GP2 and store the result in ir_2.distance.cm distanceRead(ir_2); // The value can be printed using %u eg rprintf("Distance=%u",ir_2.distance.cm); // or dumped using: rprintf("ir_2: "); distanceDump(ir_2); rprintfCRLF(); // -------- End Sharp GP2------- // -------- Start Sharp GP2------- // Read the Sharp GP2 and store the result in ir_3.distance.cm distanceRead(ir_3); // The value can be printed using %u eg rprintf("Distance=%u",ir_3.distance.cm); // or dumped using: rprintf("ir_3: "); distanceDump(ir_3); rprintfCRLF(); // -------- End Sharp GP2------- // -------- Start Generic I2C Device------- // Read 3 register values starting at register 1 into response uint8_t lbRTOS_response[3]; if(i2cMasterReadRegisters(&lbRTOS.i2cInfo, 1, sizeof(lbRTOS_response), lbRTOS_response)){ // We have successfully read the data into 'lbRTOS_response' }else{ rprintf("Failed reading lbRTOS\n"); } // -------- End Generic I2C Device------- return 0; }
// This is the main loop TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart) { //rprintf("\t\t---------------------COUNTER: %d\n", counter); // -------- Start Switch/Button------- // Switch/Button - see switch.h // To test if it is pressed then if(SWITCH_pressed(&button)){ // pressed } // To test if it is released then if(SWITCH_released(&button)){ // released } // -------- End Switch/Button------- // -------- Start Marquee------- // Marquee - see 'segled.h' // Before using the Marquee you need to redirect rprintf to write to it // This can be done using Writer old = rprintfInit(marqueeGetWriter(&marquee)); // All rprintf output will then be sent to the marquee but will not // display until an end-of-line eg "\n" has been sent. Example:- // rprintf("Hello World\n"); // If the endDelay is non-zero then the marquee will scroll // forever or until you call: marqueeStop(&marquee); // If the endDelay is zero then the marquee will stop once // the entire line has been shown ('one-shot' mode) // In 'one-shot' mode then you may want to make sure that // a previous line has finished before you display a second line. // This can be done as follows:- marqueeSetEndDelay(&marquee,0); // Make sure we are in one-shot mode if(marqueeIsActive(&marquee)==FALSE){ if(loopCount==1){ rprintf("ABCDEFGHIJKLMNOPQRSTUVWXYZ\n"); }else{ rprintf("Loop=%u\n",(unsigned)loopCount); // Put the loop count } } // Restore rprintf back to its previous location rprintfInit(old); // -------- End Marquee------- /* FIGURE EIGHT TEST for(uint8_t i = 0; i < 90; i++){ send_angle(cmd_r); delay_ms(62); rprintf("lb: %d\n",robot.sen_heading); cmd_r++; } for(uint8_t i = 0; i < 180; i++){ send_angle(cmd_r); delay_ms(62); rprintf("lb: %d\n",robot.sen_heading); cmd_r--; } for(uint8_t i = 0; i < 90; i++){ send_angle(cmd_r); delay_ms(62); rprintf("lb: %d\n",robot.sen_heading); cmd_r++; } */ // rprintf("lb vel: %d\n",robot.sen_vel); //rprintf("MAIN LOOP\n"); // lclz_plot(); //ir_filter(); send_angle(5); // -------- Start Sharp GP2------- // -90 degrees // Read the Sharp GP2 and store the result in ir_0.distance.cm //distanceRead(ir_0); //rprintf("P %d\n",robot.sen_heading); // The value can be printed using %u eg rprintf("Distance=%u",ir_0.distance.cm); // or dumped using: //rprintf("ir_0: "); //distanceDump(ir_0); //rprintfCRLF(); //x = (int)( (double)(ir_0.distance.cm / 2) * cos((-M_PI / 2) + robot.sen_heading) ) + 24; //y = (int)( (double)(ir_0.distance.cm / 2) * sin((-M_PI / 2) + robot.sen_heading) ) + 24; //g_map[x][y] = 1; //rprintf("%d,%d,1\n", x, y); //x, y,, intensity // -------- End Sharp GP2------- // -------- Start Sharp GP2------- // 0 degrees // Read the Sharp GP2 and store the result in ir_1.distance.cm //distanceRead(ir_1); // The value can be printed using %u eg rprintf("Distance=%u",ir_1.distance.cm); // or dumped using: //rprintf("ir_1: "); //distanceDump(ir_1); //rprintfCRLF(); //x = (int)( (double)(ir_1.distance.cm / 2) * cos(0 + robot.sen_heading) ) + 24; //y = (int)( (double)(ir_0.distance.cm / 2) * sin(0 + robot.sen_heading) ) + 24; //g_map[x][y] = 1; //rprintf("%d,%d,1\n", x, y);//x, y,, intensity // -------- End Sharp GP2------- // -------- Start Sharp GP2------- // 90 degrees // Read the Sharp GP2 and store the result in ir_2.distance.cm //distanceRead(ir_2); // The value can be printed using %u eg rprintf("Distance=%u",ir_2.distance.cm); // or dumped using: //rprintf("ir_2: "); //distanceDump(ir_2); //rprintfCRLF(); //x = (int)( (double)(ir_2.distance.cm / 2) * cos((M_PI / 2) + robot.sen_heading) ) + 24; //y = (int)( (double)(ir_2.distance.cm / 2) * sin((M_PI / 2) + robot.sen_heading) ) + 24; //g_map[x][y] = 1; //rprintf("%d,%d,1\n", x, y);//x, y,, intensity // -------- End Sharp GP2------- /**/ // -------- Start Sharp GP2------- //45 degrees // Read the Sharp GP2 and store the result in ir_3.distance.cm //distanceRead(ir_3); // The value can be printed using %u eg rprintf("Distance=%u",ir_3.distance.cm); // or dumped using: //rprintf("\tir_3: "); //distanceDump(ir_3); //rprintfCRLF(); // -------- End Sharp GP2------- //-45 degrees //distanceRead(ir_4); //rprintf("\tir_4: "); //distanceDump(ir_4); //iter += 0.02; //print_map(); counter++; return 0; }