Revision d607fcef
| devices/DiWheelDrive/DiWheelDrive.cpp | ||
|---|---|---|
| 2 | 2 |
#include "hal.h" |
| 3 | 3 |
#include "qei.h" |
| 4 | 4 |
#include "DiWheelDrive.h" |
| 5 |
#include <chprintf.h> |
|
| 5 | 6 |
|
| 6 | 7 |
|
| 7 | 8 |
#include <global.hpp> |
| ... | ... | |
| 118 | 119 |
return RDY_OK; |
| 119 | 120 |
} |
| 120 | 121 |
break; |
| 121 |
case CAN::SET_LINE_FOLLOW_STRATEGY: |
|
| 122 |
case CAN::SET_LINE_FOLLOW_MSG: |
|
| 123 |
chprintf((BaseSequentialStream*) &SD1, "Received Strategy!\n"); |
|
| 122 | 124 |
if (frame->DLC == 1) {
|
| 123 | 125 |
global.lfStrategy = frame->data8[0]; |
| 126 |
global.msgReceived = true; |
|
| 124 | 127 |
return RDY_OK; |
| 125 | 128 |
} |
| 126 | 129 |
break; |
| ... | ... | |
| 246 | 249 |
frame.DLC = 6; |
| 247 | 250 |
this->transmitMessage(&frame); |
| 248 | 251 |
|
| 252 |
|
|
| 253 |
|
|
| 249 | 254 |
// Send the board ID (board ID of DiWheelDrive = Robot ID) |
| 250 | 255 |
if (this->bcCounter % 10 == 0) {
|
| 251 | 256 |
frame.SID = 0; |
| devices/DiWheelDrive/global.hpp | ||
|---|---|---|
| 206 | 206 |
// 3 DOCKING |
| 207 | 207 |
int lfStrategy = 3; |
| 208 | 208 |
|
| 209 |
// CAN communication line: |
|
| 210 |
// Will be set to true when message was received |
|
| 211 |
bool msgReceived = false; |
|
| 209 | 212 |
// Debugging stuff -------------- |
| 210 | 213 |
int forwardSpeed = 10; |
| 211 | 214 |
int enableRecord = 0; |
| 212 | 215 |
|
| 216 |
bool triggerCan = false; |
|
| 217 |
uint8_t strategyTest = 0; |
|
| 218 |
|
|
| 213 | 219 |
|
| 214 | 220 |
|
| 215 | 221 |
// Buffer for sensor values |
| devices/DiWheelDrive/main.cpp | ||
|---|---|---|
| 1170 | 1170 |
|
| 1171 | 1171 |
|
| 1172 | 1172 |
void setGlobalStrategy(BaseSequentialStream *chp, int argc, char *argv[]){
|
| 1173 |
uint8_t strategy = 0; |
|
| 1173 | 1174 |
if(argc == 1){
|
| 1174 |
|
|
| 1175 |
global.lfStrategy = atoi(argv[0]); |
|
| 1175 |
strategy = atoi(argv[0]); |
|
| 1176 | 1176 |
} |
| 1177 |
// send over can |
|
| 1178 |
global.strategyTest = strategy; |
|
| 1179 |
global.triggerCan = true; |
|
| 1177 | 1180 |
} |
| 1178 | 1181 |
|
| 1182 |
|
|
| 1183 |
|
|
| 1179 | 1184 |
void getPosition(BaseSequentialStream *chp, int argc, char *argv[]){
|
| 1180 | 1185 |
types::position pos = global.odometry.getPosition(); |
| 1181 | 1186 |
chprintf(chp, "Start: Position X: %d, Y: %d, Rotation X: %d, Y: %d, Z: %d\n", global.startPos.x, global.startPos.y, global.startPos.f_x, global.startPos.f_y, global.startPos.f_z); |
| devices/DiWheelDrive/userthread.cpp | ||
|---|---|---|
| 15 | 15 |
CHARGING |
| 16 | 16 |
}; |
| 17 | 17 |
|
| 18 |
enum msg_content{
|
|
| 19 |
STOP, |
|
| 20 |
START, |
|
| 21 |
EDGE_LEFT, |
|
| 22 |
EDGE_RIGHT, |
|
| 23 |
FUZZY, |
|
| 24 |
DOCKING |
|
| 25 |
}; |
|
| 26 |
|
|
| 18 | 27 |
// User thread state: |
| 19 | 28 |
states utState = states::FOLLOW_LINE; |
| 20 | 29 |
|
| ... | ... | |
| 147 | 156 |
int rpmSpeed[2] = {0};
|
| 148 | 157 |
int stop[2] = {0};
|
| 149 | 158 |
int proxyWheelThresh = 18000; |
| 159 |
LineFollowStrategy lStrategy; |
|
| 150 | 160 |
for (uint8_t led = 0; led < 8; ++led) {
|
| 151 | 161 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
| 152 | 162 |
} |
| ... | ... | |
| 184 | 194 |
this->sleep(1000); |
| 185 | 195 |
lightAllLeds(Color::BLACK); |
| 186 | 196 |
running = !running; |
| 197 |
// If message was received handle it here: |
|
| 198 |
} else if(global.msgReceived){
|
|
| 199 |
|
|
| 200 |
global.msgReceived = false; |
|
| 201 |
running = true; |
|
| 202 |
switch(global.lfStrategy){
|
|
| 203 |
case msg_content::START: |
|
| 204 |
break; |
|
| 205 |
case msg_content::STOP: |
|
| 206 |
running = false; |
|
| 207 |
break; |
|
| 208 |
case msg_content::EDGE_RIGHT: |
|
| 209 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
|
| 210 |
break; |
|
| 211 |
case msg_content::EDGE_LEFT: |
|
| 212 |
lStrategy = LineFollowStrategy::EDGE_LEFT; |
|
| 213 |
break; |
|
| 214 |
case msg_content::FUZZY: |
|
| 215 |
lStrategy = LineFollowStrategy::FUZZY; |
|
| 216 |
break; |
|
| 217 |
case msg_content::DOCKING: |
|
| 218 |
utState = states::OCCUPY; |
|
| 219 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
|
| 220 |
break; |
|
| 221 |
default: |
|
| 222 |
break; |
|
| 223 |
} |
|
| 224 |
|
|
| 187 | 225 |
} |
| 226 |
|
|
| 188 | 227 |
if(running){
|
| 189 | 228 |
switch(utState){
|
| 190 | 229 |
case states::FOLLOW_LINE:{
|
| 191 |
LineFollowStrategy lStrategy = determineStrategy(); |
|
| 230 |
// LineFollowStrategy lStrategy = determineStrategy();
|
|
| 192 | 231 |
if(lStrategy == LineFollowStrategy::DOCKING){
|
| 193 | 232 |
utState = states::OCCUPY; |
| 194 | 233 |
break; |
| ... | ... | |
| 230 | 269 |
} |
| 231 | 270 |
|
| 232 | 271 |
// Set correct following |
| 233 |
lf.setStrategy(LineFollowStrategy::EDGE_RIGHT); |
|
| 272 |
// lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
|
|
| 234 | 273 |
lf.followLine(rpmSpeed); |
| 235 | 274 |
setRpmSpeed(rpmSpeed); |
| 236 | 275 |
|
| ... | ... | |
| 240 | 279 |
break; |
| 241 | 280 |
} |
| 242 | 281 |
case states::CHARGING:{
|
| 282 |
setRpmSpeed(stop); |
|
| 243 | 283 |
break; |
| 244 | 284 |
} |
| 245 | 285 |
default:{
|
Also available in: Unified diff