amiro-os / devices / DiWheelDrive / userthread.cpp @ 9c46b728
History | View | Annotate | Download (6.376 KB)
1 | 58fe0e0b | Thomas Schöpping | #include "userthread.hpp" |
---|---|---|---|
2 | |||
3 | #include "global.hpp" |
||
4 | |||
5 | c76baf23 | Georg Alberding | #include "linefollow2.hpp" |
6 | |||
7 | 58fe0e0b | Thomas Schöpping | using namespace amiro; |
8 | |||
9 | extern Global global;
|
||
10 | |||
11 | 5d138bca | galberding | enum class states{ |
12 | FOLLOW_LINE, |
||
13 | OCCUPY, |
||
14 | RELEASE, |
||
15 | CHARGING |
||
16 | 58fe0e0b | Thomas Schöpping | }; |
17 | |||
18 | 5d138bca | galberding | // User thread state:
|
19 | states utState = states::FOLLOW_LINE; |
||
20 | 58fe0e0b | Thomas Schöpping | |
21 | // a buffer for the z-value of the accelerometer
|
||
22 | int16_t accel_z; |
||
23 | 5d138bca | galberding | bool running = false; |
24 | 58fe0e0b | Thomas Schöpping | |
25 | int vcnl4020AmbientLight[4] = {0}; |
||
26 | int vcnl4020Proximity[4] = {0}; |
||
27 | |||
28 | c76baf23 | Georg Alberding | |
29 | 58fe0e0b | Thomas Schöpping | // Set the speed by the array
|
30 | void setRpmSpeed(const int (&rpmSpeed)[2]) { |
||
31 | 5d138bca | galberding | global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
32 | 58fe0e0b | Thomas Schöpping | } |
33 | |||
34 | 5d138bca | galberding | void lightOneLed(Color color, int idx){ |
35 | global.robot.setLightColor(idx, Color(color)); |
||
36 | } |
||
37 | 58fe0e0b | Thomas Schöpping | |
38 | 5d138bca | galberding | void lightAllLeds(Color color){
|
39 | int led = 0; |
||
40 | for(led=0; led<8; led++){ |
||
41 | lightOneLed(color, led); |
||
42 | } |
||
43 | 58fe0e0b | Thomas Schöpping | } |
44 | |||
45 | 9c46b728 | galberding | /**
|
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 | 58fe0e0b | Thomas Schöpping | |
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 | 5d138bca | galberding | /*
|
145 | * SETUP
|
||
146 | */
|
||
147 | 9c46b728 | galberding | int rpmSpeed[2] = {0}; |
148 | int stop[2] = {0}; |
||
149 | int proxyWheelThresh = 18000; |
||
150 | 5d138bca | galberding | 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 | 58fe0e0b | Thomas Schöpping | /*
|
161 | * read accelerometer z-value
|
||
162 | */
|
||
163 | accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z); |
||
164 | |||
165 | /*
|
||
166 | * evaluate the accelerometer
|
||
167 | */
|
||
168 | c76baf23 | Georg Alberding | |
169 | 5d138bca | galberding | //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 | 9c46b728 | galberding | 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 | 5d138bca | galberding | |
209 | 9c46b728 | galberding | // 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 | 58fe0e0b | Thomas Schöpping | |
255 | 5d138bca | galberding | this->sleep(CAN::UPDATE_PERIOD);
|
256 | } |
||
257 | 58fe0e0b | Thomas Schöpping | |
258 | return RDY_OK;
|
||
259 | } |