int main(void) { initRP6M256(); initLCD(); writeString_P_WIFI("\n\nRP6 CONTROL M32 I2C Master Example Program!\n"); writeString_P_WIFI("\nMoving...\n"); // --------------------------------------- WDT_setRequestHandler(watchDogRequest); // --------------------------------------- // Init TWI Interface: I2CTWI_initMaster(100); I2CTWI_setRequestedDataReadyHandler(I2C_requestedDataReady); I2CTWI_setTransmissionErrorHandler(I2C_transmissionError); setLEDs(0b1111); showScreenLCD("################", "################"); mSleep(1000); showScreenLCD("I2C-Master", "Movement..."); mSleep(1000); setLEDs(0b0000); // --------------------------------------- I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_ACS_POWER, ACS_PWR_MED); I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_WDT, true); I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_WDT_RQ, true); while(true) { setLEDs(0b1001); showScreenLCD("MOVE", "FWD"); writeString_P_WIFI("\nMoving Forwards...\n"); move(70, FWD, DIST_MM(300), BLOCKING); setLEDs(0b1000); showScreenLCD("ROTATE", "LEFT"); writeString_P_WIFI("\nRotating Left...\n"); rotate(60, LEFT, 180, BLOCKING); setLEDs(0b1001); showScreenLCD("MOVE", "FWD"); writeString_P_WIFI("\nMoving Forwards...\n"); move(70, FWD, DIST_MM(300), BLOCKING); setLEDs(0b0001); showScreenLCD("ROTATE", "RIGHT"); writeString_P_WIFI("\nRotating Right...\n"); rotate(60, RIGHT, 180, BLOCKING); } return 0; }
/** * This function processes the movement commands that the behaviours generate. * Depending on the values in the behaviour_command_t struct, it sets motor * speed, moves a given distance or rotates. */ void moveCommand(behaviour_command_t * cmd) { if(cmd->move_value > 0) // move or rotate? { writeString("cmd->move_value > 0\n"); if(cmd->rotate){ rotate(cmd->speed_left, cmd->dir, cmd->move_value, false); writeString("rotate\n"); }else if(cmd->move){ move(cmd->speed_left, cmd->dir, DIST_MM(cmd->move_value), false); writeString("move\n"); } cmd->move_value = 0; // clear move value - the move commands are only // given once and then runs in background. } else if(!(cmd->move || cmd->rotate)) // just move at speed? { changeDirection(cmd->dir); moveAtSpeed(cmd->speed_left,cmd->speed_right); } else if(isMovementComplete()) // movement complete? --> clear flags! { writeString("mouvement complete\n"); cmd->rotate = false; cmd->move = false; } }
void moveCommand(behaviour_command_t * cmd) { if(cmd->move_value > 0) { if(cmd->rotate) rotate(cmd->speed_left, cmd->dir, cmd->move_value, false); else if(cmd->move) move(cmd->speed_left, cmd->dir, DIST_MM(cmd->move_value), false); cmd->move_value = 0; } else if(!(cmd->move || cmd->rotate)) { changeDirection(cmd->dir); moveAtSpeed(cmd->speed_left,cmd->speed_right); } else if(isMovementComplete()) { cmd->rotate = false; cmd->move = false; } }
void OpenLoopController(int16_t alpha,uint16_t pho, int16_t beta) { motioncomplete = 1; uint8_t dir; uint16_t angle = 0; if (abs(alpha) > 0){ dir = alpha < 0 ? LEFT : RIGHT; angle = abs(alpha); rotate(60, dir,angle, true); } writeIntegerLength(pho, DEC,16); move(80, FWD, DIST_MM(pho*10),true); beta = -alpha; if (abs(beta) > 0){ dir = beta < 0 ? LEFT : RIGHT; angle = abs(beta); rotate(60, dir, angle, true); } //VERY VERY BAD AS IT IS A OPEN LOOP CONTROLLER motioncomplete = 0; }
int main(void) { initRobotBase(); setLEDs(0b111111); mSleep(2500); powerON(); // Turn on Encoders, Current sensing, ACS and Power LED. /* In the following code segment we use the functions void move(uint8_t desired_speed, uint8_t dir, uint16_t distance, uint8_t blocking) and void rotate(uint8_t desired_speed, uint8_t dir, uint16_t angle, uint8_t blocking) The parameters have the following purposes: move: uint8_t desired_speed: The Speed the robot should try to maintain all the time during the movement. (but it will slow down a bit before it reaches the desired distance) uint8_t dir: The Direction the robot should move - this can be FWD for Forwards or BWD for Backwards. You can also use LEFT or RIGHT here, but this function is not optimized for rotation - you should use rotate instead! uint16_t distance: The distance we want to move. The distance needs to be given in encoder counts. You can use the macro DIST_MM() to convert the distance from mm to encoder counts if you have calibrated your Encoder sensors resoultion. Maximum distance is 65000 encoder counts! (not 65535 like you might have thought. There needs to be a bit space to the top value... anyways - who wants to move about 15m? ;) ) uint8_t blocking: This parameter sets if the function shall block the normal program flow during the movement. If the parameter is "true", the function will NOT quit until the robot has moved the given distance! ACS, Bumper, RC5 and other events are still processed and not affected, but the main program will be blocked (s. below). You can use "BLOCKING" instead of "true" and "NON_BLOCKING" instead of false to make the code more readable! rotate: uint8_t desired_speed: Same as above. uint8_t dir: Nearly the same as above, but here you should only use LEFT or RIGHT as parameters even if you can pass BWD or FWD to this function. uint16_t angle The angle to turn. Attention: This rotation function is not optimal for very small angles - it is optimized to turn angles over 25°! If you want smaller angles, you need to write your own function! And even for large angles this function is far away from beeing perfect as the encoder errors for rotation are rather big. You need to do some experiments to optimize this for the surfaces you want to use the Robot on. The encoders are mainly intended for very good speed regulation and don't work too well for tracked vehicles' dead reckoning. Not only for RP6 but for EVERY tracked vehicle out there - even for those with $10.000 price tag. Measuring distances when moving straight forward works OK - but not when rotating as there is a lot of tracks slippery and you always don't move as far as you measured. Solution: Use external sensors for rotation! Like electronic compass, gyroscopes, optical mouse sensor or similar. Also s. comments in next example! To block or not to block: If you set the "blocking" parameter to false, then the function will only set all necessary parameters and quit - it will not wait until the movement has finished. This code: move(60, FWD, DIST_MM(300), false); // <-- non blocking rotate(50, LEFT, 180, false); // <-- blocking mSleep(5000); would NOT work as intended! The Robot would NOT move 300mm and then turn 180°, instead it would only turn 180°! And, if you remove the mSleep(5000) at the end, it would maybe not even do this. You always need to check if the movement has finished and if you can initiate the next movement if you set the blocking parameter to false! This: move(60, FWD, DIST_MM(300), true); // <-- blocking movement rotate(50, LEFT, 180, true); // <-- blocking rotation would work like intended and the robot would move 300mm, turn 180° and stop. This is equivalent to this: move(60, FWD, DIST_MM(300), false); // <-- non-blocking movement while(!isMovementComplete()) // Now we need to wait until the movement task_RP6System(); // is finished before we go on. // If you set blocking to true, the function // does exactly this automatically. rotate(50, LEFT, 180, false); // <-- non-blocking rotation while(!isMovementComplete()) // Again, we need to wait for the task_RP6System(); // movement (rotation) to be finished... Of course, you do not need to wait for the movement to finish in a while-loop - you can also check this in several different ways and you can do other things while the movement is in progress. You will see some other ways in the next examples. You can use the "isMovementComplete()" function for checking if the last movement is complete. There is also a function "stop()" that can be used to abort a movement, e.g. if one of the sensors has detected an obstacle. But you do not need to use stop, you can also initiate the next movement without performing "stop" before. */ // Ok, now back to the example program. If you want this to work accurate, you // need to calibrate the encoders (s. RP6 manual)! // Main loop while(true) { setLEDs(0b100100); /* move(60, FWD, DIST_MM(300), true); // Move 300mm forwards // at Speed 60*5 ~= 300 counts/second // = 7.5cm/s with 0.25mm resolution // (6.9cm/s with 0.23mm resolution...) setLEDs(0b100000); rotate(50, LEFT, 180, true); // rotate 180° left setLEDs(0b100100); move(60, FWD, DIST_MM(300), true); // move 30cm forwards setLEDs(0b000100); rotate(50, RIGHT, 180, true); // rotate 180° right */ moveAtSpeed(0,0); //Bremsen move(70, BWD, DIST_MM(150), BLOCKING); //zurückfahren rotate(40, RIGHT, 30, BLOCKING); //Drehen move(70, FWD, DIST_MM(200), BLOCKING);//Vorwärts (am Hindernis vorbei) fahren rotate(40, LEFT, 30, BLOCKING); //zurückdrehen //dann geradeaus weiter } return 0; }