void CBOT_main( void ) { STEPPER_open(); // Open STEPPER module for use. // Move BOTH wheels forward and // make the robot move in a Figure 8. STEPPER_move_stwt( STEPPER_BOTH, STEPPER_FWD, 3341.106, 334.1106, 400, STEPPER_BRK_OFF, // Left STEPPER_FWD, 2305.88, 230.588, 400, STEPPER_BRK_OFF ); // Right STEPPER_move_stwt( STEPPER_BOTH, STEPPER_FWD, 2305.88, 230.588, 400, STEPPER_BRK_OFF, // Left STEPPER_FWD, 3341.106, 334.1106, 400, STEPPER_BRK_OFF ); // Right // Infinite loop! while( 1 ); }
void rotateDeg(float degree) { // Rotates a given angle in degrees (positive is CCW) if(degree > 0){ // Rotate CCW int steps = ceil(C_CCW*degree); STEPPER_move_stwt( STEPPER_BOTH, STEPPER_REV, steps, SPEED, 0, STEPPER_BRK_OFF, STEPPER_FWD, steps, SPEED, 0, STEPPER_BRK_OFF ); }else if(degree < 0){ //Rotate CW int steps = (-1)*round(C_CW*degree); STEPPER_move_stwt( STEPPER_BOTH, STEPPER_FWD, steps, SPEED, 0, STEPPER_BRK_OFF, STEPPER_REV, steps, SPEED, 0, STEPPER_BRK_OFF ); } // Update predicted position variable roboangle += degree; //updateLCD(); }
void forward(float dist) { // Moves forwards or backwards a distance "dist" given in feet. int Lsteps = ceil(C_FWD_LW*dist)*12; int Rsteps = ceil(C_FWD_RW*dist)*12; if(dist>0){ //Move Forwards STEPPER_move_stwt( STEPPER_BOTH, STEPPER_FWD, Lsteps, SPEED, 0, STEPPER_BRK_OFF, STEPPER_FWD, Rsteps, SPEED, 0, STEPPER_BRK_OFF ); }else if(dist<0){ STEPPER_move_stwt( STEPPER_BOTH, STEPPER_REV, -1*Lsteps, SPEED, 0, STEPPER_BRK_OFF, STEPPER_REV, -1*Rsteps, SPEED, 0, STEPPER_BRK_OFF ); } // Update predicted position variables roboxpos += dist * cos(roboangle*M_PI/180); roboypos += dist * sin(roboangle*M_PI/180); }
/******************************************************************* * Function: char move_arc_stwt(float, float, float, float, BOOL) * Input Variables: char * Output Return: float, float, float, float, BOOL * Overview: This moves the robot in any arc length ********************************************************************/ char move_arc_stwt(float arc_Radius, float arc_Length, float arc_Speed, float arc_Accel, BOOL arc_Brk) { BOOL step_Fwd_L = (arc_Length>0); BOOL step_Fwd_R = (arc_Length>0); float step_Num = abs(arc_Length/D_STEP); float step_Speed = abs(arc_Speed/D_STEP); float step_Accel = abs(arc_Accel/D_STEP); if(arc_Radius == NO_TURN){ STEPPER_move_stwt( STEPPER_BOTH, step_Fwd_L, step_Num, step_Speed, step_Accel, arc_Brk, // Left step_Fwd_R, step_Num, step_Speed, step_Accel, arc_Brk); // Right return SUCCESS; } if(arc_Radius == POINT_TURN){ STEPPER_move_stwt( STEPPER_BOTH, !step_Fwd_L, step_Num, step_Speed, step_Accel, arc_Brk, // Left step_Fwd_R, step_Num, step_Speed, step_Accel, arc_Brk); // Right return SUCCESS; } float arc_Length_L; float arc_Length_R; float arc_Speed_L; float arc_Speed_R; float arc_Accel_L; float arc_Accel_R; float step_Num_L; float step_Num_R; float step_Speed_L; float step_Speed_R; float step_Accel_L; float step_Accel_R; if(arc_Radius > 0){ arc_Length_L = arc_Length * (1 - WHEEL_BASE/arc_Radius); arc_Length_R = arc_Length * (1 + WHEEL_BASE/arc_Radius); step_Num_L = arc_Length_L/D_STEP; step_Num_R = arc_Length_R/D_STEP; arc_Speed_L = arc_Speed * (1 - WHEEL_BASE/arc_Radius); arc_Speed_R = arc_Speed * (1 + WHEEL_BASE/arc_Radius); step_Speed_L = arc_Speed_L/D_STEP; step_Speed_R = arc_Speed_R/D_STEP; arc_Accel_L = arc_Accel * (1 - WHEEL_BASE/arc_Radius); arc_Accel_R = arc_Accel * (1 + WHEEL_BASE/arc_Radius); step_Accel_L = arc_Accel_L/D_STEP; step_Accel_R = arc_Accel_R/D_STEP; STEPPER_move_stwt( STEPPER_BOTH, step_Fwd_L, step_Num_L, step_Speed_L, step_Accel_L, arc_Brk, // Left step_Fwd_R, step_Num_R, step_Speed_R, step_Accel_R, arc_Brk); // Right return SUCCESS; } if(arc_Radius < 0){ arc_Length_L = arc_Length * (1 + WHEEL_BASE/arc_Radius); arc_Length_R = arc_Length * (1 - WHEEL_BASE/arc_Radius); step_Num_L = arc_Length_L/D_STEP; step_Num_R = arc_Length_R/D_STEP; arc_Speed_L = arc_Speed * (1 + WHEEL_BASE/arc_Radius); arc_Speed_R = arc_Speed * (1 - WHEEL_BASE/arc_Radius); step_Speed_L = arc_Speed_L/D_STEP; step_Speed_R = arc_Speed_R/D_STEP; arc_Accel_L = arc_Accel * (1 + WHEEL_BASE/arc_Radius); arc_Accel_R = arc_Accel * (1 - WHEEL_BASE/arc_Radius); step_Accel_L = arc_Accel_L/D_STEP; step_Accel_R = arc_Accel_R/D_STEP; STEPPER_move_stwt( STEPPER_BOTH, step_Fwd_L, step_Num_L, step_Speed_L, step_Accel_L, arc_Brk, // Left step_Fwd_R, step_Num_R, step_Speed_R, step_Accel_R, arc_Brk); // Right return SUCCESS; } return FAIL; }
/******************************************************************* * Function: char moveRetreat(void) * Input Variables: void * Output Return: char * Overview: Moves robot backwards until contact sensors encounter a wall or obstacle ********************************************************************/ char moveRetreat( void ) { // Make a variable that keeps track of this behavior char isRetreat = 0; // Back up until an object is encountered // Check for left and right contact if((rightContact != 1)&&(leftContact != 1)) { // move backward while nothing is detected STEPPER_move_stnb( STEPPER_BOTH, STEPPER_FWD, 200, 200, 450, STEPPER_BRK_OFF, // Left STEPPER_FWD, 200, 200, 450, STEPPER_BRK_OFF ); // Right isRetreat = 1; } // Check right contact else if((leftContact == 0)&&(rightContact == 1)) { // move forward to leave room for turn STEPPER_move_stwt( STEPPER_BOTH, STEPPER_REV, 75, 200, 250, STEPPER_BRK_OFF, // Left STEPPER_REV, 75, 200, 250, STEPPER_BRK_OFF ); // Right // Turn 1/4 of a revolution CW STEPPER_move_stwt( STEPPER_BOTH, STEPPER_REV, 100, 200, 250, STEPPER_BRK_OFF, // Left STEPPER_FWD, 100, 200, 250, STEPPER_BRK_OFF ); // Right retreatFlagStatus = 0; } // Check left contact else if((leftContact == 1)&&(rightContact == 0)) { // move forward to leave room for turn STEPPER_move_stwt( STEPPER_BOTH, STEPPER_REV, 75, 200, 250, STEPPER_BRK_OFF, // Left STEPPER_REV, 75, 200, 250, STEPPER_BRK_OFF ); // Right // Turn 1/4 of a revolution CCW STEPPER_move_stwt( STEPPER_BOTH, STEPPER_FWD, 100, 200, 250, STEPPER_BRK_OFF, // Left STEPPER_REV, 100, 200, 250, STEPPER_BRK_OFF ); // Right retreatFlagStatus = 0; } // check both contacts else if ((leftContact == 1)&&(rightContact == 1)) { // move forward to leave room for turn STEPPER_move_stwt( STEPPER_BOTH, STEPPER_REV, 75, 200, 250, STEPPER_BRK_OFF, // Left STEPPER_REV, 75, 200, 250, STEPPER_BRK_OFF ); // Right // Turn 1/4 of a revolution CCW STEPPER_move_stwt( STEPPER_BOTH, STEPPER_FWD, 100, 200, 250, STEPPER_BRK_OFF, // Left STEPPER_REV, 100, 200, 250, STEPPER_BRK_OFF ); // Right retreatFlagStatus = 0; } return isRetreat; }