void turn_direction(float direction)
{
int end_turn;
if (direction < PI) // turn clockwise
{
e_set_steps_left(0);
e_set_speed_left(TURN_SPEED); // motor speed in steps/s
e_set_speed_right(-TURN_SPEED); // motor speed in steps/s
// calculate how many steps the robot needs to turn
end_turn = (int)(STEPS_FOR_2PI*(direction/(2*PI)));
while(e_get_steps_left() < end_turn){
flick_led();
} // turn until done
}
else // turn counterclockwise
{
e_set_steps_right(0);
e_set_speed_left(-TURN_SPEED); // motor speed in steps/s
e_set_speed_right(TURN_SPEED); // motor speed in steps/s
// calculate how many steps the robot needs to turn
end_turn = (int)(STEPS_FOR_2PI*((2*PI-direction)/(2*PI)));
while(e_get_steps_right() < end_turn){
flick_led();
} // turn until done
}
// stop motors
e_set_speed_left(0); // motor speed in steps/s
e_set_speed_right(0); // motor speed in steps/s
}
/** Stop the motor activities
*
* Set the speed for zero and clean the step counter
*/
void stop_motors()
{
e_set_speed_right(0);
e_set_speed_left(0);
e_set_steps_right(0);
e_set_steps_left(0);
send_char(1);/* It signalize the end of operation. */
}
void send_steps()
{
int steps_right,steps_left;
steps_right = e_get_steps_right();
steps_left = e_get_steps_left();
e_set_steps_right(0);
e_set_steps_left(0);
ve_send_int(steps_right);
ve_send_int(steps_left);
}
void run_square() {
unsigned char useGyroFlag = 0; // 0=use odometry, 1=use gyroscope
unsigned char squareState = 0;
signed int tempZ = 0;
float turnAngle = 0;
float diffTime = 0;
float dpmsValue = 0;
//e_init_uart2(BAUD230400); // for debug
while(1) {
if(isEpuckVersion1_1() && useGyroFlag==1) {
switch(squareState) {
case 0: // set speed to go forward (square side)
e_set_speed_left(150);
e_set_speed_right(150);
e_set_steps_left(0);
squareState = 1;
break;
case 1: // go forward for a while
if(e_get_steps_left() >= 800) {
squareState = 2;
}
break;
case 2: // set speed to rotate on the square angle
e_set_steps_left(0);
e_set_speed_left(-100);
e_set_speed_right(100);
resetTime();
turnAngle = 0;
squareState = 3;
break;
case 3: // rotate 90 degrees
tempZ = getZAxisGyro();
dpmsValue = rawToDpms(tempZ);
diffTime = getDiffTimeMsAndReset();
turnAngle += diffTime*dpmsValue; // integrate the angular rate to get the rotation angle
//sprintf(buffer, "%f,%d,%f\r\n", turnAngle,tempZ,diffTime);
//e_send_uart2_char(buffer, strlen(buffer));
if(turnAngle >= 90) {
e_set_speed_left(0);
e_set_speed_right(0);
squareState = 0;
}
break;
}
} else {
switch(squareState) {
case 0: // set speed to go forward (square side)
e_set_speed_left(150);
e_set_speed_right(150);
e_set_steps_left(0);
squareState = 1;
break;
case 1: // go forward for a while
if(e_get_steps_left() >= 800) {
squareState = 2;
}
break;
case 2: // set speed to rotate on the square angle
e_set_steps_left(0);
e_set_speed_left(-100);
e_set_speed_right(100);
resetTime();
turnAngle = 0;
squareState = 3;
break;
case 3: // rotate 90 degrees
turnAngle = e_get_steps_left();
//sprintf(buffer, "%f,%d,%f\r\n", turnAngle,tempZ,diffTime);
//e_send_uart2_char(buffer, strlen(buffer));
if(turnAngle <= -330) { // from field test 330 steps correspond to about 90 degrees
e_set_speed_left(0);
e_set_speed_right(0);
squareState = 0;
}
break;
}
}
}
}
