Revision 9c46b728
| devices/DiWheelDrive/DiWheelDrive.cpp | ||
|---|---|---|
| 3 | 3 |
#include "qei.h" |
| 4 | 4 |
#include "DiWheelDrive.h" |
| 5 | 5 |
|
| 6 |
|
|
| 6 | 7 |
#include <global.hpp> |
| 7 | 8 |
|
| 8 | 9 |
using namespace chibios_rt; |
| ... | ... | |
| 98 | 99 |
} |
| 99 | 100 |
break; |
| 100 | 101 |
case CAN::SET_LINE_FOLLOW_SPEED: |
| 101 |
if (frame->DLC == 8) {
|
|
| 102 |
uint8_t speedForward = frame->data8[0]; |
|
| 103 |
uint8_t speedSoftLeft0 = frame->data8[1]; |
|
| 104 |
uint8_t speedSoftLeft1 = frame->data8[2]; |
|
| 105 |
uint8_t speedHardLeft0 = frame->data8[3]; |
|
| 106 |
uint8_t speedHardLeft1 = frame->data8[4]; |
|
| 107 |
global.rpmForward[0] = speedForward; |
|
| 108 |
global.rpmForward[1] = speedForward; |
|
| 109 |
global.rpmSoftLeft[0] = speedSoftLeft0; |
|
| 110 |
global.rpmSoftLeft[1] = speedSoftLeft1; |
|
| 111 |
global.rpmHardLeft[0] = speedHardLeft0; |
|
| 112 |
global.rpmHardLeft[1] = speedHardLeft1; |
|
| 113 |
global.rpmSoftRight[0] = global.rpmSoftLeft[1]; |
|
| 114 |
global.rpmSoftRight[1] = global.rpmSoftLeft[0]; |
|
| 115 |
global.rpmHardRight[0] = global.rpmHardLeft[1]; |
|
| 116 |
global.rpmHardRight[1] = global.rpmHardLeft[0]; |
|
| 117 |
return RDY_OK; |
|
| 118 |
} |
|
| 102 |
if (frame->DLC == 8) {
|
|
| 103 |
uint8_t speedForward = frame->data8[0]; |
|
| 104 |
uint8_t speedSoftLeft0 = frame->data8[1]; |
|
| 105 |
uint8_t speedSoftLeft1 = frame->data8[2]; |
|
| 106 |
uint8_t speedHardLeft0 = frame->data8[3]; |
|
| 107 |
uint8_t speedHardLeft1 = frame->data8[4]; |
|
| 108 |
global.rpmForward[0] = speedForward; |
|
| 109 |
global.rpmForward[1] = speedForward; |
|
| 110 |
global.rpmSoftLeft[0] = speedSoftLeft0; |
|
| 111 |
global.rpmSoftLeft[1] = speedSoftLeft1; |
|
| 112 |
global.rpmHardLeft[0] = speedHardLeft0; |
|
| 113 |
global.rpmHardLeft[1] = speedHardLeft1; |
|
| 114 |
global.rpmSoftRight[0] = global.rpmSoftLeft[1]; |
|
| 115 |
global.rpmSoftRight[1] = global.rpmSoftLeft[0]; |
|
| 116 |
global.rpmHardRight[0] = global.rpmHardLeft[1]; |
|
| 117 |
global.rpmHardRight[1] = global.rpmHardLeft[0]; |
|
| 118 |
return RDY_OK; |
|
| 119 |
} |
|
| 120 |
break; |
|
| 121 |
case CAN::SET_LINE_FOLLOW_STRATEGY: |
|
| 122 |
if (frame->DLC == 1) {
|
|
| 123 |
global.lfStrategy = frame->data8[0]; |
|
| 124 |
return RDY_OK; |
|
| 125 |
} |
|
| 126 |
break; |
|
| 119 | 127 |
case CAN::SET_KINEMATIC_CONST_ID: |
| 120 | 128 |
if (frame->DLC == 8) {
|
| 121 | 129 |
/* // Set (but do not store) Ed |
| devices/DiWheelDrive/global.hpp | ||
|---|---|---|
| 28 | 28 |
#include <DiWheelDrive.h> |
| 29 | 29 |
#include <userthread.hpp> |
| 30 | 30 |
|
| 31 |
|
|
| 31 | 32 |
using namespace amiro; |
| 32 | 33 |
|
| 33 | 34 |
class Global final |
| ... | ... | |
| 163 | 164 |
|
| 164 | 165 |
DiWheelDrive robot; |
| 165 | 166 |
|
| 167 |
|
|
| 168 |
|
|
| 166 | 169 |
UserThread userThread; |
| 167 | 170 |
int rpmForward[2] = {20,20};
|
| 168 | 171 |
int rpmSoftLeft[2] = {10,20};
|
| ... | ... | |
| 189 | 192 |
|
| 190 | 193 |
// Struct for saving motor gains |
| 191 | 194 |
|
| 192 |
int resetProtect = 1; |
|
| 193 |
motorGains motorConfigGains; |
|
| 194 |
motorGains stopGains; |
|
| 195 |
int resetProtect = 1; |
|
| 196 |
motorGains motorConfigGains; |
|
| 197 |
motorGains stopGains; |
|
| 198 |
|
|
| 199 |
types::position startPos; |
|
| 200 |
types::position endPos; |
|
| 195 | 201 |
|
| 196 |
types::position startPos; |
|
| 197 |
types::position endPos; |
|
| 202 |
// Line Following strategy |
|
| 203 |
// 0 EDGE_RIGHT |
|
| 204 |
// 1 EDGE_LEFT |
|
| 205 |
// 2 FUZZY |
|
| 206 |
// 3 DOCKING |
|
| 207 |
int lfStrategy = 3; |
|
| 198 | 208 |
|
| 199 | 209 |
// Debugging stuff -------------- |
| 200 | 210 |
int forwardSpeed = 10; |
| devices/DiWheelDrive/linefollow2.hpp | ||
|---|---|---|
| 2 | 2 |
#define AMIRO_LINEFOLLOWING_H |
| 3 | 3 |
|
| 4 | 4 |
#include <ch.hpp> |
| 5 |
|
|
| 5 |
#include "global.hpp" |
|
| 6 | 6 |
#include <amiroosconf.h> |
| 7 | 7 |
|
| 8 | 8 |
namespace amiro {
|
| 9 |
|
|
| 9 | 10 |
enum class LineFollowStrategy{
|
| 10 | 11 |
EDGE_LEFT, |
| 11 | 12 |
EDGE_RIGHT, |
| 12 | 13 |
DOCKING, |
| 13 | 14 |
MIDDLE, |
| 14 |
FUZZY |
|
| 15 |
}; |
|
| 15 |
FUZZY, |
|
| 16 |
NONE |
|
| 17 |
}; |
|
| 16 | 18 |
|
| 17 | 19 |
enum colorMember : uint8_t {
|
| 18 | 20 |
BLACK=0, |
| devices/DiWheelDrive/main.cpp | ||
|---|---|---|
| 1160 | 1160 |
|
| 1161 | 1161 |
} |
| 1162 | 1162 |
|
| 1163 |
void enableCharging(){
|
|
| 1164 |
global.ltc4412.enable(true); |
|
| 1165 |
} |
|
| 1163 | 1166 |
|
| 1164 |
void getAccel(BaseSequentialStream *chp, int argc, char *argv[]){
|
|
| 1165 |
int steps = 0; |
|
| 1166 |
// global.motorcontrol.getGains(&global.motorConfigGains); |
|
| 1167 |
// global.motorcontrol.setGains(&global.stopGains); |
|
| 1168 |
int16_t time = 10000; |
|
| 1169 |
int32_t angle = 3141592; |
|
| 1170 |
int32_t distance = 0; |
|
| 1171 |
if (argc == 1){
|
|
| 1172 |
chprintf(chp, "%i steps \n", atoi(argv[0])); |
|
| 1173 |
steps = atoi(argv[0]); |
|
| 1174 |
} |
|
| 1175 |
else if(argc == 3){
|
|
| 1176 |
distance = atoi(argv[0]); |
|
| 1177 |
angle = atoi(argv[0]); |
|
| 1178 |
time = atoi(argv[0]); |
|
| 1179 |
}else{
|
|
| 1180 |
chprintf(chp, "Use: accel <steps>\n"); |
|
| 1181 |
return; |
|
| 1182 |
} |
|
| 1183 |
global.distcontrol.setTargetPosition(distance, angle, time); |
|
| 1167 |
void disableCharging(){
|
|
| 1168 |
global.ltc4412.enable(false); |
|
| 1169 |
} |
|
| 1184 | 1170 |
|
| 1185 |
for(int i=0; i<steps; i++){
|
|
| 1186 |
int16_t Z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z); |
|
| 1187 |
int16_t X = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_X); |
|
| 1188 |
int16_t Y = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Y); |
|
| 1189 |
types::position pos = global.odometry.getPosition(); |
|
| 1190 |
// chprintf((BaseSequentialStream*)&global.sercanmux1, "Axis X: %d, Y: %d, Z: %d\n", X, Y, Z); |
|
| 1191 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Position X: %d, Y: %d, Rotation X: %d, Y: %d, Z: %d, Angle: %d\n", pos.x, pos.y, pos.f_x, pos.f_y, pos.f_z, global.distcontrol.getCurrentTargetAngle()); |
|
| 1192 |
// chprintf((BaseSequentialStream*)&global.sercanmux1, "Position X: %d, Y: %d, Z: %d\n", pos.f_x, pos.f_y, pos.f_z); |
|
| 1193 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 1194 |
} |
|
| 1195 | 1171 |
|
| 1196 |
// global.motorcontrol.setGains(&global.motorConfigGains); |
|
| 1172 |
void setGlobalStrategy(BaseSequentialStream *chp, int argc, char *argv[]){
|
|
| 1173 |
if(argc == 1){
|
|
| 1174 |
|
|
| 1175 |
global.lfStrategy = atoi(argv[0]); |
|
| 1176 |
} |
|
| 1197 | 1177 |
} |
| 1198 | 1178 |
|
| 1199 | 1179 |
void getPosition(BaseSequentialStream *chp, int argc, char *argv[]){
|
| ... | ... | |
| 1210 | 1190 |
} |
| 1211 | 1191 |
} |
| 1212 | 1192 |
|
| 1193 |
// TODO: Not wokring, either loading station has no power or logic not working |
|
| 1194 |
void checkPower(BaseSequentialStream *chp, int argc, char *argv[]){
|
|
| 1195 |
int steps = 2000; |
|
| 1196 |
int led = 0; |
|
| 1197 |
enableCharging(); |
|
| 1198 |
for (int i=0; i<steps;i++){
|
|
| 1199 |
chprintf(chp, "%s Enable: %s\n", global.ltc4412.isPluggedIn() ? "y" : "n", global.ltc4412.isEnabled() ? "y2" : "n2"); |
|
| 1200 |
if(global.ltc4412.isPluggedIn()){
|
|
| 1201 |
// enableCharging(); |
|
| 1202 |
for(led=0; led<8; led++){
|
|
| 1203 |
global.robot.setLightColor(led, Color(Color::GREEN)); |
|
| 1204 |
} |
|
| 1205 |
}else{
|
|
| 1206 |
// disableCharging(); |
|
| 1207 |
for(led=0; led<8; led++){
|
|
| 1208 |
global.robot.setLightColor(led, Color(Color::RED)); |
|
| 1209 |
} |
|
| 1210 |
} |
|
| 1211 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 1212 |
} |
|
| 1213 |
disableCharging(); |
|
| 1214 |
for(led=0; led<8; led++){
|
|
| 1215 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
|
| 1216 |
} |
|
| 1217 |
} |
|
| 1218 |
|
|
| 1219 |
|
|
| 1213 | 1220 |
static const ShellCommand commands[] = {
|
| 1214 | 1221 |
{"shutdown", shellRequestShutdown},
|
| 1215 | 1222 |
{"wakeup", shellRequestWakeup},
|
| ... | ... | |
| 1249 | 1256 |
{"followLine", followLine},
|
| 1250 | 1257 |
{"rotate", rotate},
|
| 1251 | 1258 |
{"followRotate", followAndRotate},
|
| 1252 |
{"accel", getAccel},
|
|
| 1253 | 1259 |
{"getPos", getPosition},
|
| 1260 |
{"checkPower", checkPower},
|
|
| 1261 |
{"setStrategy", setGlobalStrategy},
|
|
| 1254 | 1262 |
{NULL, NULL}
|
| 1255 | 1263 |
}; |
| 1256 | 1264 |
|
| devices/DiWheelDrive/userthread.cpp | ||
|---|---|---|
| 42 | 42 |
} |
| 43 | 43 |
} |
| 44 | 44 |
|
| 45 |
/** |
|
| 46 |
* Get the wanted strategy from the global object. |
|
| 47 |
*/ |
|
| 48 |
LineFollowStrategy determineStrategy(){
|
|
| 49 |
switch(global.lfStrategy){
|
|
| 50 |
case 0: |
|
| 51 |
return LineFollowStrategy::EDGE_RIGHT; |
|
| 52 |
break; |
|
| 53 |
case 1: |
|
| 54 |
return LineFollowStrategy::EDGE_LEFT; |
|
| 55 |
break; |
|
| 56 |
case 2: |
|
| 57 |
return LineFollowStrategy::FUZZY; |
|
| 58 |
break; |
|
| 59 |
case 3: |
|
| 60 |
return LineFollowStrategy::DOCKING; |
|
| 61 |
default: |
|
| 62 |
break; |
|
| 63 |
} |
|
| 64 |
return LineFollowStrategy::NONE; |
|
| 65 |
} |
|
| 66 |
|
|
| 67 |
/** |
|
| 68 |
* Blocks as long as the position changes. |
|
| 69 |
*/ |
|
| 70 |
void checkForMotion(UserThread *ut){
|
|
| 71 |
int motion = 1; |
|
| 72 |
int led = 0; |
|
| 73 |
types::position oldPos = global.odometry.getPosition(); |
|
| 74 |
while(motion){
|
|
| 75 |
ut->sleep(500); |
|
| 76 |
types::position tmp = global.odometry.getPosition(); |
|
| 77 |
motion = abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z); |
|
| 78 |
oldPos = tmp; |
|
| 79 |
global.robot.setLightColor((led + 1) % 8, Color(Color::YELLOW)); |
|
| 80 |
global.robot.setLightColor(led % 8, Color(Color::BLACK)); |
|
| 81 |
led++; |
|
| 82 |
} |
|
| 83 |
} |
|
| 84 |
|
|
| 85 |
|
|
| 86 |
/** |
|
| 87 |
* Turns MotorControl off and checks if position remains stable. |
|
| 88 |
*/ |
|
| 89 |
int checkDockingSuccess(UserThread *ut){
|
|
| 90 |
// global.motorcontrol.setTargetRPM(0,0); |
|
| 91 |
checkForMotion(ut); |
|
| 92 |
int led = 0; |
|
| 93 |
int success = 0; |
|
| 94 |
global.odometry.resetPosition(); |
|
| 95 |
types::position start = global.startPos = global.odometry.getPosition(); |
|
| 96 |
global.motorcontrol.toggleMotorEnable(); |
|
| 97 |
ut->sleep(1000); |
|
| 98 |
types::position stop = global.endPos = global.odometry.getPosition(); |
|
| 99 |
global.motorcontrol.toggleMotorEnable(); |
|
| 100 |
// Amiro moved, docking was not successful |
|
| 101 |
if ((start.x + stop.x) || (start.y + stop.y)){
|
|
| 102 |
for(led=0; led<8; led++){
|
|
| 103 |
global.robot.setLightColor(led, Color(Color::RED)); |
|
| 104 |
} |
|
| 105 |
success = 0; |
|
| 106 |
}else{
|
|
| 107 |
for(led=0; led<8; led++){
|
|
| 108 |
global.robot.setLightColor(led, Color(Color::GREEN)); |
|
| 109 |
} |
|
| 110 |
success = 1; |
|
| 111 |
} |
|
| 112 |
|
|
| 113 |
ut->sleep(500); |
|
| 114 |
for(led=0; led<8; led++){
|
|
| 115 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
|
| 116 |
} |
|
| 117 |
return success; |
|
| 118 |
} |
|
| 119 |
|
|
| 120 |
void correctPosition(UserThread *ut, LineFollow &lf, int steps){
|
|
| 121 |
int checkWhite = 0; |
|
| 122 |
int rpmSpeed[2] ={0};
|
|
| 123 |
lf.setStrategy(LineFollowStrategy::EDGE_LEFT); |
|
| 124 |
for (int correction=0; correction<steps; correction++){
|
|
| 125 |
checkWhite = lf.followLine(rpmSpeed); |
|
| 126 |
setRpmSpeed(rpmSpeed); |
|
| 127 |
ut->sleep(CAN::UPDATE_PERIOD); |
|
| 128 |
} |
|
| 129 |
} |
|
| 45 | 130 |
|
| 46 | 131 |
|
| 47 | 132 |
UserThread::UserThread() : |
| ... | ... | |
| 59 | 144 |
/* |
| 60 | 145 |
* SETUP |
| 61 | 146 |
*/ |
| 62 |
int rpmFuzzyCtrl[2] = {0};
|
|
| 147 |
int rpmSpeed[2] = {0};
|
|
| 148 |
int stop[2] = {0};
|
|
| 149 |
int proxyWheelThresh = 18000; |
|
| 63 | 150 |
for (uint8_t led = 0; led < 8; ++led) {
|
| 64 | 151 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
| 65 | 152 |
} |
| ... | ... | |
| 99 | 186 |
running = !running; |
| 100 | 187 |
} |
| 101 | 188 |
if(running){
|
| 189 |
switch(utState){
|
|
| 190 |
case states::FOLLOW_LINE:{
|
|
| 191 |
LineFollowStrategy lStrategy = determineStrategy(); |
|
| 192 |
if(lStrategy == LineFollowStrategy::DOCKING){
|
|
| 193 |
utState = states::OCCUPY; |
|
| 194 |
break; |
|
| 195 |
} |
|
| 196 |
lf.setStrategy(lStrategy); |
|
| 197 |
lf.followLine(rpmSpeed); |
|
| 198 |
setRpmSpeed(rpmSpeed); |
|
| 199 |
break; |
|
| 200 |
} |
|
| 201 |
case states::OCCUPY:{
|
|
| 202 |
// Get Wheel proxy sensors |
|
| 203 |
int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset(); |
|
| 204 |
int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset(); |
|
| 205 |
// Check for black |
|
| 206 |
if ((WL+WR) < proxyWheelThresh){ /*TODO: Only check for one sensor */
|
|
| 207 |
global.motorcontrol.setTargetRPM(0,0); |
|
| 102 | 208 |
|
| 103 |
} |
|
| 209 |
// Stop when in docking state and take further actions |
|
| 210 |
if(lf.getStrategy() == LineFollowStrategy::DOCKING){
|
|
| 211 |
if(checkDockingSuccess(this)){
|
|
| 212 |
utState = states::CHARGING; |
|
| 213 |
break; |
|
| 214 |
}else{
|
|
| 215 |
correctPosition(this, lf, 250); |
|
| 216 |
lf.setStrategy(LineFollowStrategy::DOCKING); |
|
| 217 |
// break; |
|
| 218 |
} |
|
| 219 |
}else{
|
|
| 220 |
checkForMotion(this); |
|
| 221 |
// 180° Rotation |
|
| 222 |
global.distcontrol.setTargetPosition(0, 3141592, 10000); |
|
| 223 |
// BaseThread::sleep(8000); |
|
| 224 |
checkForMotion(this); |
|
| 225 |
correctPosition(this, lf, 250); |
|
| 226 |
// break; |
|
| 227 |
lf.setStrategy(LineFollowStrategy::DOCKING); |
|
| 228 |
|
|
| 229 |
} |
|
| 230 |
} |
|
| 231 |
|
|
| 232 |
// Set correct following |
|
| 233 |
lf.setStrategy(LineFollowStrategy::EDGE_RIGHT); |
|
| 234 |
lf.followLine(rpmSpeed); |
|
| 235 |
setRpmSpeed(rpmSpeed); |
|
| 236 |
|
|
| 237 |
break; |
|
| 238 |
} |
|
| 239 |
case states::RELEASE:{
|
|
| 240 |
break; |
|
| 241 |
} |
|
| 242 |
case states::CHARGING:{
|
|
| 243 |
break; |
|
| 244 |
} |
|
| 245 |
default:{
|
|
| 246 |
break; |
|
| 247 |
} |
|
| 248 |
} |
|
| 249 |
|
|
| 250 |
}else{
|
|
| 251 |
// Stop |
|
| 252 |
setRpmSpeed(stop); |
|
| 253 |
} |
|
| 104 | 254 |
|
| 105 |
lf.followLine(rpmFuzzyCtrl); |
|
| 106 |
setRpmSpeed(rpmFuzzyCtrl); |
|
| 107 |
// this->sleep(US2ST(5)); |
|
| 108 | 255 |
this->sleep(CAN::UPDATE_PERIOD); |
| 109 | 256 |
} |
| 110 | 257 |
|
Also available in: Unified diff