controller - 为所需导航设计 Kobuki 机器人
问题描述
我们被要求设计 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 度然后左转。
解决方案
推荐阅读
- angularjs - 使用角度重复分页(角度 1.2)
- c# - 对数组中元素的多次引用与将该元素分配给变量
- android - java.lang.VerifyError:验证程序拒绝了类 kotlin.UByte
- vue.js - 如何修复 vue.js 中的 BrowserslistError?
- python - How to extract a substring from a string in python between a marker and the nth occurrence of another marker
- python - 到达数据框中的每个单元格
- asp.net-mvc - 如何在数据源读取方法后更新(重新加载)剑道网格的列?
- firebase - Firebase orderBy 不起作用(当 orderBy 字段相同时)
- java - 询问非原始数组列表
- python - 张量流中的可训练参数是什么?