amiro-os / devices / DiWheelDrive / userthread.cpp @ 9c46b728
History | View | Annotate | Download (6.38 KB)
1 |
#include "userthread.hpp" |
---|---|
2 |
|
3 |
#include "global.hpp" |
4 |
|
5 |
#include "linefollow2.hpp" |
6 |
|
7 |
using namespace amiro; |
8 |
|
9 |
extern Global global;
|
10 |
|
11 |
enum class states{ |
12 |
FOLLOW_LINE, |
13 |
OCCUPY, |
14 |
RELEASE, |
15 |
CHARGING |
16 |
}; |
17 |
|
18 |
// User thread state:
|
19 |
states utState = states::FOLLOW_LINE; |
20 |
|
21 |
// a buffer for the z-value of the accelerometer
|
22 |
int16_t accel_z; |
23 |
bool running = false; |
24 |
|
25 |
int vcnl4020AmbientLight[4] = {0}; |
26 |
int vcnl4020Proximity[4] = {0}; |
27 |
|
28 |
|
29 |
// Set the speed by the array
|
30 |
void setRpmSpeed(const int (&rpmSpeed)[2]) { |
31 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
32 |
} |
33 |
|
34 |
void lightOneLed(Color color, int idx){ |
35 |
global.robot.setLightColor(idx, Color(color)); |
36 |
} |
37 |
|
38 |
void lightAllLeds(Color color){
|
39 |
int led = 0; |
40 |
for(led=0; led<8; led++){ |
41 |
lightOneLed(color, led); |
42 |
} |
43 |
} |
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 |
} |
130 |
|
131 |
|
132 |
UserThread::UserThread() : |
133 |
chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>() |
134 |
{ |
135 |
} |
136 |
|
137 |
UserThread::~UserThread() |
138 |
{ |
139 |
} |
140 |
|
141 |
msg_t |
142 |
UserThread::main() |
143 |
{ |
144 |
/*
|
145 |
* SETUP
|
146 |
*/
|
147 |
int rpmSpeed[2] = {0}; |
148 |
int stop[2] = {0}; |
149 |
int proxyWheelThresh = 18000; |
150 |
for (uint8_t led = 0; led < 8; ++led) { |
151 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
152 |
} |
153 |
running = false;
|
154 |
LineFollow lf(&global); |
155 |
/*
|
156 |
* LOOP
|
157 |
*/
|
158 |
while (!this->shouldTerminate()) |
159 |
{ |
160 |
/*
|
161 |
* read accelerometer z-value
|
162 |
*/
|
163 |
accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z); |
164 |
|
165 |
/*
|
166 |
* evaluate the accelerometer
|
167 |
*/
|
168 |
|
169 |
//Features over can: (Line Following)
|
170 |
//SetKP
|
171 |
//SetKI
|
172 |
//SetKd ?
|
173 |
//SetSleepTime
|
174 |
//SeForwardSpeed? or SetMaxSpeed
|
175 |
//DriveOnLeftLine
|
176 |
//DriveOnRightLine
|
177 |
|
178 |
//if accel_z<-900
|
179 |
//send can event (one time)
|
180 |
// Line following is started if amiro roteted
|
181 |
if (accel_z < -900 /*-0.9g*/) { //new: (can.allowLineFollowOnTurn && accel_z < 900) || can.startLineFollow optional feature |
182 |
// set the front LEDs to blue for one second
|
183 |
lightAllLeds(Color::GREEN); |
184 |
this->sleep(1000); |
185 |
lightAllLeds(Color::BLACK); |
186 |
running = !running; |
187 |
} |
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); |
208 |
|
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 |
} |
254 |
|
255 |
this->sleep(CAN::UPDATE_PERIOD);
|
256 |
} |
257 |
|
258 |
return RDY_OK;
|
259 |
} |