int16_t main(void) { initRobotBase(); powerON(); setLEDs(0b111111); mSleep(1000); setLEDs(0b000000); mSleep(500); uint8_t runningLight = 1; for (uint8_t i = 0; i < 6; i++){ for (uint8_t j = 0; j< 3;j++) destination[j] = destinationarray[i][j]; //mainloopOpen(); mainloopClosed(); fancyled(&runningLight); /*for (uint8_t j = 0; j< 3;j++) source[j] = destination[j];*/ mSleep(2000); } task_RP6System(); while(true) { fancyled(&runningLight); if (motioncomplete == 0) moveAtSpeed(0,0); mSleep(200); task_RP6System(); } return 0; }
void mainloopOpen() { motioncomplete=1; while(motioncomplete == 1) { double delta_x=0.0, delta_y=0.0,pho=0.0; //Odometry(); //should comment for open loop controller delta_x = destination[0] - source[0]; delta_y = destination[1] - source[1]; pho = sqrt(pow(delta_x,2)+pow(delta_y,2)); double alpha = atan2(delta_y,delta_x); if (abs(alpha*100) > 0){ alpha = (alpha*180)/M_PI; //alpha = (alpha < 0 ? -(alpha +180):alpha); } /*writeIntegerLength((int16_t)delta_y, DEC,16); writeString("\n"); writeIntegerLength((int16_t)delta_x, DEC,16); writeString("\n"); writeIntegerLength((int16_t)alpha, DEC,16); writeString("\n");*/ task_RP6System(); /*REPLACE THIS CONTROLLER*/ OpenLoopController((int16_t)alpha,pho,0); //SiegwartController(alpha,pho,0); task_RP6System(); //motioncomplete=0; //debug ... do not forget to remove :-) mSleep(200); } }
void mainloopClosed() { motioncomplete = 1; while(motioncomplete == 1){ double delta_x=0.0, delta_y=0.0,pho=0.0; Odometry(); delta_x = destination[0] - source[0]; delta_y = destination[1] - source[1]; pho = sqrt(pow(delta_x,2)+pow(delta_y,2)); double alpha = atan2(delta_y,delta_x); /*if (abs(alpha*100) > 0){ alpha = (alpha*180)/M_PI; }*/ task_RP6System(); SiegwartController(alpha,pho,0); task_RP6System(); task_motionControl(); mSleep(200); } }
int main(void) { initRobotBase(); setLEDs(0b111111); mSleep(1500); setLEDs(0b000000); // Set Bumpers state changed event handler: BUMPERS_setStateChangedHandler(bumpersStateChanged); powerON(); // Turn Encoders, Motor Current Sensors // ATTENTION: Automatic Motor control will not work without this! /* RP6 SAGAN GENERATED COMMANDS START */ /*{SAGAN1_COMMANDS_HERE}*/ /* RP6 SAGAN GENERATED COMMANDS STOP */ stop(); moveAtSpeed(0, 0); BUMPERS_setStateChangedHandler(BUMPERS_stateChanged_empty); setLEDs(0b000000); while (true) { statusLEDs.LED2 = !statusLEDs.LED2; // Toggle LED bit in LED shadow register... statusLEDs.LED5 = !statusLEDs.LED5; updateStatusLEDs(); mSleep(500); task_RP6System(); } return 0; }
int main(void) { initRobotBase(); setLEDs(0b111111); mSleep(2500); setLEDs(0b100100); BUMPERS_setStateChangedHandler(bumpersStateChanged); powerON(); while(true) { behaviourController(); task_RP6System(); } return 0; }
/** * Here we react on any obstacle that we may hit. * If any of the bumpers detects an obstacle, we stop the motors and start * LED blink. */ void bumpersStateChanged(void) { if (bumper_left || bumper_right) { stop(); moveAtSpeed(0, 0); // stop moving! setLEDs(0b111111); updateStatusLEDs(); mSleep(500); while (true) { statusLEDs.LED2 = !statusLEDs.LED2; // Toggle LED bit in LED shadow register... statusLEDs.LED5 = !statusLEDs.LED5; updateStatusLEDs(); mSleep(500); task_RP6System(); } } }