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;
}
