Нас попросили разработать код C для робота, чтобы он двигался прямо, когда нет препятствий. В случае препятствия, робот должен повернуться на 180 градусов и повернуть налево.
Во время восхождения на гору робот должен идти в гору и вниз, если нет препятствия. В случае обнаружения обрыва робот должен повернуть на 180, а затем повернуть налево.
Мы попробовали код для запрашиваемого дизайна. Мы смогли заставить робота двигаться;Обнаружение препятствий работает нормально. Даже подъем на гору все хорошо, за исключением скорости. Но обнаружение обрыва по какой-то причине полностью останавливает робота.
/*
* KobukiNavigationStatechart.c
*
*/
#include "kobukiNavigationStatechart.h"
#include <math.h>
#include <stdlib.h>
// Program States
typedef enum{
INITIAL = 0, // Initial state
PAUSE_WAIT_BUTTON_RELEASE, // Paused; pause button pressed down, wait until released before detecting next press
UNPAUSE_WAIT_BUTTON_PRESS, // Paused; wait for pause button to be pressed
UNPAUSE_WAIT_BUTTON_RELEASE, // Paused; pause button pressed down, wait until released before returning to previous state
DRIVE, // Drive straight
TURNL, /// Turn
TURNR, /// TURN Left
DRIVER,
DRIVEFIX,
TURNUPR,
TURNUPL,
TURNDOWNL,
TURNDOWNR,
HILLUP,
HILLDOWN,
DRIVEUP,
DRIVEDOWN,
} robotState_t;
#define DEG_PER_RAD (180.0 / M_PI) // degrees per radian
#define RAD_PER_DEG (M_PI / 180.0) // radians per degree
void KobukiNavigationStatechart(
const int16_t maxWheelSpeed,
const int32_t netDistance,
const int32_t netAngle,
const KobukiSensors_t sensors,
const accelerometer_t accelAxes,
int16_t * const pRightWheelSpeed,
int16_t * const pLeftWheelSpeed,
const bool isSimulator
){
// local state
static robotState_t state = INITIAL; // current program state
static robotState_t unpausedState = DRIVE; // state history for pause region
static int32_t distanceAtManeuverStart = 0; // distance robot had travelled when a maneuver begins, in mm
static int32_t angleAtManeuverStart = 0; // angle through which the robot had TURNed when a maneuver begins, in deg
int drive_state = 0;
// outputs
int16_t leftWheelSpeed = 0; // speed of the left wheel, in mm/s
int16_t rightWheelSpeed = 0; // speed of the right wheel, in mm/s
//*****************************************************
// state data - process inputs *
//*****************************************************
if (state == INITIAL
|| state == PAUSE_WAIT_BUTTON_RELEASE
|| state == UNPAUSE_WAIT_BUTTON_PRESS
|| state == UNPAUSE_WAIT_BUTTON_RELEASE
|| sensors.buttons.B0 // pause button
){
switch (state){
case INITIAL:
// set state data that may change between simulation and real-world
if (isSimulator){
}
else{
}
state = UNPAUSE_WAIT_BUTTON_PRESS; // place into pause state
break;
case PAUSE_WAIT_BUTTON_RELEASE:
// remain in this state until released before detecting next press
if (!sensors.buttons.B0){
state = UNPAUSE_WAIT_BUTTON_PRESS;
}
break;
case UNPAUSE_WAIT_BUTTON_RELEASE:
// user pressed 'pause' button to reTURN to previous state
if (!sensors.buttons.B0){
state = unpausedState;
}
break;
case UNPAUSE_WAIT_BUTTON_PRESS:
// remain in this state until user presses 'pause' button
if (sensors.buttons.B0){
state = UNPAUSE_WAIT_BUTTON_RELEASE;
}
break;
default:
// must be in run region, and pause button has been pressed
unpausedState = state;
state = PAUSE_WAIT_BUTTON_RELEASE;
break;
}
}
//*************************************
// state transition - run region *
//*************************************
//DRIVE until sensor
else if (state == DRIVE && (sensors.bumps_wheelDrops.bumpCenter| sensors.bumps_wheelDrops.bumpRight| sensors.bumps_wheelDrops.bumpLeft|sensors.cliffCenter|sensors.cliffLeft|sensors.cliffRight)){
angleAtManeuverStart = netAngle;
distanceAtManeuverStart = netDistance;
state = DRIVER;
}//DRIVEUP until sensor
else if (state == DRIVEUP && (sensors.bumps_wheelDrops.bumpCenter | sensors.bumps_wheelDrops.bumpRight | sensors.bumps_wheelDrops.bumpLeft)) {
angleAtManeuverStart = netAngle;
distanceAtManeuverStart = netDistance;
state = DRIVER;
}//DRIVEDOWN until sensor
else if (state == DRIVEDOWN && (sensors.bumps_wheelDrops.bumpCenter | sensors.bumps_wheelDrops.bumpRight | sensors.bumps_wheelDrops.bumpLeft | sensors.cliffCenter | sensors.cliffLeft | sensors.cliffRight)) {
angleAtManeuverStart = netAngle;
distanceAtManeuverStart = netDistance;
state = DRIVER;
}//DRIVE until decline
else if (state == DRIVE && (accelAxes.y <-0.05 | (accelAxes.x < -0.05) | (accelAxes.x > 0.05))&&drive_state==1) {
angleAtManeuverStart = netAngle;
distanceAtManeuverStart = netDistance;
state = HILLDOWN;
drive_state = 2;
}//DRIVE until incline
else if (state == DRIVE && (accelAxes.y <-0.05 | (accelAxes.x < -0.05)|(accelAxes.x > 0.05))) {
angleAtManeuverStart = netAngle;
distanceAtManeuverStart = netDistance;
state = HILLUP;
drive_state = 1;
}
//DRIVEUP UNTIL FLAT
else if (state == DRIVEUP && (sensors.cliffCenter | sensors.cliffLeft | sensors.cliffRight)) {(accelAxes.y > -0.05 && accelAxes.y < 0.05 && ((accelAxes.x > -0.05) && (accelAxes.x < 0.05))
angleAtManeuverStart = netAngle;
distanceAtManeuverStart = netDistance;
state = TURNDOWNL;
drive_state = 2;
}//DRIVEDOWN UNTIL FLAT
else if (state == DRIVEDOWN && (accelAxes.y > -0.05 && accelAxes.y < 0.05 && ((accelAxes.x > -0.05) && (accelAxes.x < 0.05)))) {
angleAtManeuverStart = netAngle;
distanceAtManeuverStart = netDistance;
state = TURNDOWNR;
drive_state = 0;
}//TURN UP RIGHT
else if (state == HILLUP && (accelAxes.x > 0)) {
angleAtManeuverStart = netAngle;
distanceAtManeuverStart = netDistance;
state = TURNUPR;
}//TURN UP LEFT
else if (state == HILLUP && (accelAxes.x < 0)) {
angleAtManeuverStart = netAngle;
distanceAtManeuverStart = netDistance;
state = TURNUPL;
}
//TURN DOWN RIGHT
else if (state == HILLDOWN && (accelAxes.x > -0.01)) {
angleAtManeuverStart = netAngle;
distanceAtManeuverStart = netDistance;
state = TURNDOWNR;
}//TURN DOWN LEFT
else if (state == HILLDOWN && (accelAxes.x < -0.01)) {
angleAtManeuverStart = netAngle;
distanceAtManeuverStart = netDistance;
state = TURNDOWNL;
}
//DRIVE UP FROM LEFT
else if (state == TURNUPL && (accelAxes.y > 0.03 && ((accelAxes.x > -0.01) && (accelAxes.x < 0.01)))) {
angleAtManeuverStart = netAngle;
distanceAtManeuverStart = netDistance;
state = DRIVEUP;
}//DRIVE UP FROM RIGHT
else if (state == TURNUPR && (accelAxes.y > 0.03 && ((accelAxes.x > -0.01) && (accelAxes.x < 0.01)))) {
angleAtManeuverStart = netAngle;
distanceAtManeuverStart = netDistance;
state = DRIVEUP;
}
//DRIVE DOWN FROM LEFT
else if ((state == TURNDOWNL||state == TURNDOWNR) && abs(netAngle - angleAtManeuverStart) >= 180) {// (accelAxes.y < -0.03 && ((accelAxes.x > -0.01) && (accelAxes.x < 0.01)))
angleAtManeuverStart = netAngle;
distanceAtManeuverStart = netDistance;
state = DRIVEDOWN;
}//PART OF FIXED OBSTACLE AVOIDING
else if (state == TURNR && abs(netAngle - angleAtManeuverStart) >= 90){
angleAtManeuverStart = netAngle;
distanceAtManeuverStart = netDistance;
state = DRIVEFIX;
}//PART OF FIXED OBSTACLE AVOIDING
else if (state == TURNL && abs(netAngle - angleAtManeuverStart) >= 90) {
angleAtManeuverStart = netAngle;
distanceAtManeuverStart = netDistance;
if(drive_state==1) state = DRIVEUP;
else if (drive_state == 2) state = DRIVEDOWN;
else state = DRIVE;
}//PART OF FIXED OBSTACLE AVOIDING
else if (state == DRIVER && abs(netDistance - distanceAtManeuverStart) >= 250) {
angleAtManeuverStart = netAngle;
distanceAtManeuverStart = netDistance;
state = TURNR;
}//PART OF FIXED OBSTACLE AVOIDING
else if (state == DRIVEFIX && (sensors.bumps_wheelDrops.bumpCenter | sensors.bumps_wheelDrops.bumpRight | sensors.bumps_wheelDrops.bumpLeft | sensors.cliffCenter | sensors.cliffLeft | sensors.cliffRight)) {
angleAtManeuverStart = netAngle;
distanceAtManeuverStart = netDistance;
state = DRIVER;
}//PART OF FIXED OBSTACLE AVOIDING
else if (state == DRIVEFIX && abs(netDistance - distanceAtManeuverStart) >= 250) {
angleAtManeuverStart = netAngle;
distanceAtManeuverStart = netDistance;
state = TURNL;
}
// else, no transitions are taken
//*****************
//* state actions *
//*****************
switch (state){
case INITIAL:
case PAUSE_WAIT_BUTTON_RELEASE:
case UNPAUSE_WAIT_BUTTON_PRESS:
case UNPAUSE_WAIT_BUTTON_RELEASE:
// in pause mode, robot should be stopped
leftWheelSpeed = rightWheelSpeed = 0;
break;
case DRIVEUP:
case DRIVEDOWN:
case DRIVE:
// full speed ahead!
leftWheelSpeed = rightWheelSpeed = 100;
break;
case DRIVEFIX:
// full speed ahead!
leftWheelSpeed = rightWheelSpeed = 100;
break;
case DRIVER:
// full speed ahead!
leftWheelSpeed = rightWheelSpeed = -100;
break;
case TURNUPR:
case TURNR:
leftWheelSpeed = 100;
rightWheelSpeed = -leftWheelSpeed;
break;
case TURNUPL:
case TURNL:
leftWheelSpeed = -100;
rightWheelSpeed = -leftWheelSpeed;
break;
case HILLUP:
case HILLDOWN:
default:
// Unknown state
leftWheelSpeed = rightWheelSpeed = 0;
}
*pLeftWheelSpeed = leftWheelSpeed;
*pRightWheelSpeed = rightWheelSpeed;
}
Мы ожидаем, что обнаружение обрыва приведет к повороту на 180 градусов, а затем повернет влево.