Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / userthread.cpp @ 05d54823

History | View | Annotate | Download (9.42 KB)

1
#include "userthread.hpp"
2
#include "global.hpp"
3
#include "linefollow.hpp" 
4

    
5
using namespace amiro;
6

    
7
extern Global global;
8

    
9
// a buffer for the z-value of the accelerometer
10
int16_t accel_z;
11
bool running = false;
12

    
13

    
14
/**
15
 * Set speed.
16
 * 
17
 * @param rpmSpeed speed for left and right wheel in rounds/min
18
 */
19
void UserThread::setRpmSpeed(const int (&rpmSpeed)[2]) {
20
  global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
21
}
22

    
23
void UserThread::lightOneLed(Color color, int idx){
24
  global.robot.setLightColor(idx, Color(color));
25
}
26

    
27
void UserThread::lightAllLeds(Color color){
28
  int led = 0;
29
  for(led=0; led<8; led++){
30
        lightOneLed(color, led);
31
      }
32
}
33

    
34
void UserThread::showChargingState(){
35
  uint8_t numLeds = global.robot.getPowerStatus().state_of_charge / 12;
36
  Color color = Color::GREEN;
37
  if (numLeds <= 2){
38
    color = Color::RED;
39
  }else if(numLeds <= 6){
40
    color = Color::YELLOW;
41
  }
42
  for (int i=0; i<numLeds; i++){
43
    lightOneLed(color, i);
44
    this->sleep(300);
45
  }
46
  this->sleep(1000);
47
  lightAllLeds(Color::BLACK);
48
}
49

    
50
/**
51
 * Blocks as long as the position changes.
52
 */
53
void UserThread::checkForMotion(){
54
  int motion = 1;
55
  int led = 0;
56
  types::position oldPos = global.odometry.getPosition();
57
  while(motion){
58
    this->sleep(500);
59
    types::position tmp = global.odometry.getPosition();
60
    motion = abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z);
61
    oldPos = tmp;
62
      global.robot.setLightColor((led + 1) % 8, Color(Color::YELLOW));
63
      global.robot.setLightColor(led % 8, Color(Color::BLACK));
64
      led++;
65
  }
66
}
67

    
68
bool UserThread::checkPinVoltage(){
69
  return global.ltc4412.isPluggedIn(); 
70
}
71

    
72
bool UserThread::checkPinEnabled(){
73
  return global.ltc4412.isEnabled();
74
}
75

    
76
int UserThread::checkDockingSuccess(){
77
  // setRpmSpeed(stop);
78
  checkForMotion();
79
  int success = 0;
80
  global.odometry.resetPosition();
81
  types::position start = global.startPos = global.odometry.getPosition();
82
  global.motorcontrol.toggleMotorEnable();
83
  this->sleep(1000);
84
  types::position stop_ = global.endPos = global.odometry.getPosition();
85
  
86
  // Amiro moved, docking was not successful
87
  if ((start.x + stop_.x) || (start.y + stop_.y)){
88
    lightAllLeds(Color::RED);
89
    // Enable Motor again if docking was not successful
90
    global.motorcontrol.toggleMotorEnable();
91
    success = 0;
92
  }else{
93
    lightAllLeds(Color::GREEN);
94
    success = 1;
95
  }
96
  
97
  this->sleep(500);
98
  lightAllLeds(Color::BLACK);
99
  return success;
100
}
101

    
102
UserThread::UserThread() :
103
  chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>()
104
{
105
}
106

    
107
UserThread::~UserThread()
108
{
109
}
110

    
111
msg_t
112
UserThread::main()
113
{
114
  /*
115
   * SETUP
116
   */
117
  // User thread state:
118
states utState = states::IDLE;
119
states newState;
120

    
121
   int whiteBuf = 0;
122
   int correctionStep = 0;
123
  int rpmSpeed[2] = {0};
124
  int stop[2] = {0};
125
  LineFollowStrategy lStrategy = LineFollowStrategy::FUZZY;
126
  for (uint8_t led = 0; led < 8; ++led) {
127
    global.robot.setLightColor(led, Color(Color::BLACK));
128
  }
129
  running = false;
130
  LineFollow lf(&global);
131
  /*
132
   * LOOP
133
   */
134
  while (!this->shouldTerminate())
135
  {
136
    /*
137
    * read accelerometer z-value
138
    */
139
    accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z);
140
        
141
    if (accel_z < -900 /*-0.9g*/) { 
142
      // Start line following when AMiRo is rotated
143
      if(utState == states::IDLE){
144
        utState = states::FOLLOW_LINE;
145
      }else{
146
        utState = states::IDLE;
147
      }
148
      lightAllLeds(Color::GREEN);
149
      this->sleep(1000);
150
      lightAllLeds(Color::BLACK);
151

    
152
    // If message was received handle it here:
153
    } else if(global.msgReceived){
154
      global.msgReceived = false;
155
      // running = true;
156
      switch(global.lfStrategy){
157
        case msg_content::START:
158
          utState = states::FOLLOW_LINE;
159
          break;
160
        case msg_content::STOP:
161
          utState = states::IDLE;
162
          break;
163
        case msg_content::EDGE_RIGHT:
164
          // utState = states::FOLLOW_LINE;
165
          lStrategy = LineFollowStrategy::EDGE_RIGHT;
166
          break;
167
        case  msg_content::EDGE_LEFT:
168
          // utState = states::FOLLOW_LINE;
169
          lStrategy = LineFollowStrategy::EDGE_LEFT;
170
          break;
171
        case msg_content::FUZZY:
172
          // utState = states::FOLLOW_LINE;
173
          lStrategy = LineFollowStrategy::FUZZY;
174
          break;
175
        case msg_content::DOCK:
176
          utState = states::DETECT_STATION;
177
          break;
178
        case msg_content::UNDOCK:
179
          utState = states::RELEASE;
180
          break;
181
        case msg_content::CHARGE:
182
          utState = states::CHARGING;
183
          break;
184
        default:
185
          utState = states::IDLE;
186
          break;
187
      }
188
    }
189
    newState = utState;
190

    
191
    // Get sensor data 
192
    int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
193
    int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
194
    // int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
195
    // int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
196
    switch(utState){
197
      case states::IDLE:
198
        if (!global.motorcontrol.getMotorEnable()){
199
            global.motorcontrol.toggleMotorEnable();
200
        }
201
        setRpmSpeed(stop);
202
        if(/* checkPinVoltage() && */ checkPinEnabled()){
203
          global.robot.requestCharging(0);
204
        }
205
        
206
        break;
207
      // ---------------------------------------
208
      case states::FOLLOW_LINE:
209
      // Set correct forward speed to every strategy
210
        if (global.forwardSpeed != global.rpmForward[0]){
211
          global.forwardSpeed = global.rpmForward[0];
212
        }
213
        
214
        if(lf.getStrategy() != lStrategy){
215
          lf.setStrategy(lStrategy);
216
        }
217

    
218
        //TODO: Check if white is detected and stop threshold is reached
219
        if(lf.followLine(rpmSpeed)){
220
          
221
          if(whiteBuf >= WHITE_COUNT_THRESH){
222
            setRpmSpeed(stop);
223
            newState = states::IDLE;
224
          }else{
225
            whiteBuf++;
226
          }
227
        }else{
228
          whiteBuf = 0;
229
          setRpmSpeed(rpmSpeed);
230
        }
231
        // lf.followLine(rpmSpeed);
232
        // setRpmSpeed(rpmSpeed);
233
        
234
        break;
235
      // ---------------------------------------
236
      case states::DETECT_STATION:
237
        if (global.forwardSpeed != CHARGING_SPEED){
238
          global.forwardSpeed = CHARGING_SPEED;
239
        }
240
        if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
241
          lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
242
        }
243

    
244
        lf.followLine(rpmSpeed);
245
        setRpmSpeed(rpmSpeed);
246
        // // Detect marker before docking station
247
        if ((WL+WR) < PROXY_WHEEL_THRESH){
248
          setRpmSpeed(stop);
249
          checkForMotion();
250
          // 180° Rotation 
251
          global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
252
          // BaseThread::sleep(8000);
253
          checkForMotion();
254
          newState = states::CORRECT_POSITIONING;
255
        }
256
        break;
257
      // ---------------------------------------
258
      case states::CORRECT_POSITIONING:
259
        if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){
260
          lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
261
        }
262
        lf.followLine(rpmSpeed);
263
        setRpmSpeed(rpmSpeed);
264

    
265
        correctionStep++;
266
        if (correctionStep >= MAX_CORRECTION_STEPS){
267
          correctionStep = 0;
268
          newState = states::REVERSE;
269
          setRpmSpeed(stop);
270
          checkForMotion();
271
        }
272
        break;
273
      // ---------------------------------------
274
      case states::REVERSE:
275
        if(lf.getStrategy() != LineFollowStrategy::REVERSE){
276
          lf.setStrategy(LineFollowStrategy::REVERSE);
277
        }
278
        lf.followLine(rpmSpeed);
279
        setRpmSpeed(rpmSpeed);
280

    
281
        if ((WL+WR) < PROXY_WHEEL_THRESH){
282
          setRpmSpeed(stop);
283
          checkForMotion();
284
          newState = states::CHECK_POSITIONING;
285
        }
286
        break;
287
      // ---------------------------------------
288
      case states::CHECK_POSITIONING:
289
        if(checkDockingSuccess()/* && checkVoltage() */){
290
          newState = states::CHARGING;
291
        }else{
292
          newState = states::CORRECT_POSITIONING;
293
        }
294
        break;
295
      // ---------------------------------------
296
      case states::CHARGING:
297
        if (global.motorcontrol.getMotorEnable()){
298
          global.motorcontrol.toggleMotorEnable();
299
        }
300
        // Formulate Request to enable charging
301
        if(/* checkPinVoltage() && */ !checkPinEnabled()){
302
          global.robot.requestCharging(1);
303
        }
304
        if(checkPinEnabled()){
305
          showChargingState();
306
        }
307
        break;
308
      
309
      // ---------------------------------------
310
      case states::RELEASE:
311
        if(/* checkPinVoltage() && */ checkPinEnabled()){
312
          global.robot.requestCharging(0);
313
        }
314

    
315
        if(checkPinEnabled()){
316
          showChargingState();
317
        }else{
318
          if (!global.motorcontrol.getMotorEnable()){
319
            global.motorcontrol.toggleMotorEnable();
320
          }
321
          //Rotate -20° to free from magnet
322
          global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
323
          checkForMotion();
324
          lStrategy = LineFollowStrategy::EDGE_RIGHT;
325
          newState = states::FOLLOW_LINE;
326
        }
327
        lightAllLeds(Color::BLACK);
328
        break;
329
      
330
      default:
331
        break;
332
      }
333
      utState = newState;
334
    this->sleep(CAN::UPDATE_PERIOD);
335
  }
336

    
337
  return RDY_OK;
338
}