Revision 181f2892

View differences:

devices/DiWheelDrive/userthread.cpp
1 1
#include "userthread.hpp"
2

  
3 2
#include "global.hpp"
4

  
5 3
#include "linefollow2.hpp" 
6 4

  
7 5
using namespace amiro;
8 6

  
9 7
extern Global global;
10 8

  
11
enum class states{
12
  FOLLOW_LINE,
13
  OCCUPY,
14
  RELEASE,
15
  CHARGING
16
};
17

  
18
enum msg_content{
19
  STOP, 
20
  START,
21
  EDGE_LEFT,
22
  EDGE_RIGHT,
23
  FUZZY,
24
  DOCKING
25
};
26

  
27
// User thread state:
28
states utState = states::FOLLOW_LINE;
29

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

  
34
int vcnl4020AmbientLight[4] = {0};
35
int vcnl4020Proximity[4] = {0};
36 13

  
37

  
38
// Set the speed by the array
39
void setRpmSpeed(const int (&rpmSpeed)[2]) {
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]) {
40 20
  global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
41 21
}
42 22

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

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

  
54
/**
55
 * Get the wanted strategy from the global object.
56
 */
57
LineFollowStrategy determineStrategy(){
58
  switch(global.lfStrategy){
59
    case 0:
60
      return LineFollowStrategy::EDGE_RIGHT;
61
      break;
62
    case 1:
63
      return LineFollowStrategy::EDGE_LEFT;
64
      break;
65
    case 2:
66
      return LineFollowStrategy::FUZZY;
67
      break;
68
    case 3:
69
      return LineFollowStrategy::DOCKING;
70
    default:
71
      break;
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);
72 45
  }
73
  return LineFollowStrategy::NONE;
46
  this->sleep(1000);
47
  lightAllLeds(Color::BLACK);
74 48
}
75 49

  
76 50
/**
77 51
 * Blocks as long as the position changes.
78 52
 */
79
void checkForMotion(UserThread *ut){
53
void UserThread::checkForMotion(){
80 54
  int motion = 1;
81 55
  int led = 0;
82 56
  types::position oldPos = global.odometry.getPosition();
83 57
  while(motion){
84
    ut->sleep(500);
58
    this->sleep(500);
85 59
    types::position tmp = global.odometry.getPosition();
86 60
    motion = abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z);
87 61
    oldPos = tmp;
......
91 65
  }
92 66
}
93 67

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

  
95
/**
96
 * Turns MotorControl off and checks if position remains stable.
97
 */
98
int checkDockingSuccess(UserThread *ut){
99
  // global.motorcontrol.setTargetRPM(0,0);
100
  checkForMotion(ut);
101
  int led = 0;
72
bool UserThread::checkPinEnabled(){
73
  return global.ltc4412.isEnabled();
74
}
75

  
76
int UserThread::checkDockingSuccess(){
77
  // setRpmSpeed(stop);
78
  checkForMotion();
102 79
  int success = 0;
103 80
  global.odometry.resetPosition();
104 81
  types::position start = global.startPos = global.odometry.getPosition();
105 82
  global.motorcontrol.toggleMotorEnable();
106
  ut->sleep(1000);
107
  types::position stop = global.endPos = global.odometry.getPosition();
108
  global.motorcontrol.toggleMotorEnable();
83
  this->sleep(1000);
84
  types::position stop_ = global.endPos = global.odometry.getPosition();
85
  
109 86
  // Amiro moved, docking was not successful
110
  if ((start.x + stop.x) || (start.y + stop.y)){
111
    for(led=0; led<8; led++){
112
      global.robot.setLightColor(led, Color(Color::RED));
113
    }
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();
114 91
    success = 0;
115 92
  }else{
116
    for(led=0; led<8; led++){
117
      global.robot.setLightColor(led, Color(Color::GREEN));
118
    }
93
    lightAllLeds(Color::GREEN);
119 94
    success = 1;
120 95
  }
121 96
  
122
  ut->sleep(500);
123
  for(led=0; led<8; led++){
124
          global.robot.setLightColor(led, Color(Color::BLACK));
125
  }
97
  this->sleep(500);
98
  lightAllLeds(Color::BLACK);
126 99
  return success;
127 100
}
128 101

  
129
void correctPosition(UserThread *ut, LineFollow &lf, int steps){
130
  int checkWhite = 0;
131
  int rpmSpeed[2] ={0};
132
  lf.setStrategy(LineFollowStrategy::EDGE_LEFT);
133
  for (int correction=0; correction<steps; correction++){
134
    checkWhite = lf.followLine(rpmSpeed);
135
    setRpmSpeed(rpmSpeed);
136
    ut->sleep(CAN::UPDATE_PERIOD);
137
  }
138
}
139

  
140

  
141 102
UserThread::UserThread() :
142 103
  chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>()
143 104
{
......
153 114
  /*
154 115
   * SETUP
155 116
   */
117
  // User thread state:
118
states utState = states::IDLE;
119
states newState;
120

  
121
   int whiteBuf = 0;
122
   int correctionStep = 0;
156 123
  int rpmSpeed[2] = {0};
157 124
  int stop[2] = {0};
158
  int proxyWheelThresh = 18000;
159
  LineFollowStrategy lStrategy;
125
  LineFollowStrategy lStrategy = LineFollowStrategy::FUZZY;
160 126
  for (uint8_t led = 0; led < 8; ++led) {
161 127
    global.robot.setLightColor(led, Color(Color::BLACK));
162 128
  }
......
167 133
   */
168 134
  while (!this->shouldTerminate())
169 135
  {
170
        /*
171
         * read accelerometer z-value
172
         */
173
        accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z);
174

  
175
        /*
176
         * evaluate the accelerometer
177
         */
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);
178 151

  
179
  //Features over can: (Line Following)
180
  //SetKP
181
  //SetKI
182
  //SetKd ?
183
  //SetSleepTime
184
  //SeForwardSpeed? or SetMaxSpeed
185
  //DriveOnLeftLine
186
  //DriveOnRightLine
187
  
188
  //if accel_z<-900
189
    //send can event (one time)
190
    // Line following is started if amiro roteted
191
    if (accel_z < -900 /*-0.9g*/) { //new: (can.allowLineFollowOnTurn && accel_z < 900) || can.startLineFollow optional feature
192
        // set the front LEDs to blue for one second
193
        lightAllLeds(Color::GREEN);
194
        this->sleep(1000);
195
        lightAllLeds(Color::BLACK);
196
        running = !running;
197 152
    // If message was received handle it here:
198 153
    } else if(global.msgReceived){
199

  
200 154
      global.msgReceived = false;
201
      running = true;
155
      // running = true;
202 156
      switch(global.lfStrategy){
203 157
        case msg_content::START:
158
          utState = states::FOLLOW_LINE;
204 159
          break;
205 160
        case msg_content::STOP:
206
          running = false;
161
          utState = states::IDLE;
207 162
          break;
208 163
        case msg_content::EDGE_RIGHT:
164
          // utState = states::FOLLOW_LINE;
209 165
          lStrategy = LineFollowStrategy::EDGE_RIGHT;
210 166
          break;
211 167
        case  msg_content::EDGE_LEFT:
168
          // utState = states::FOLLOW_LINE;
212 169
          lStrategy = LineFollowStrategy::EDGE_LEFT;
213 170
          break;
214 171
        case msg_content::FUZZY:
172
          // utState = states::FOLLOW_LINE;
215 173
          lStrategy = LineFollowStrategy::FUZZY;
216 174
          break;
217
        case msg_content::DOCKING:
218
          utState = states::OCCUPY;
219
          lStrategy = LineFollowStrategy::EDGE_RIGHT;
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;
220 183
          break;
221 184
        default:
185
          utState = states::IDLE;
222 186
          break;
223 187
      }
224
    
225 188
    }
189
    newState = utState;
226 190

  
227
    if(running){
228
      switch(utState){
229
        case states::FOLLOW_LINE:{
230
          // LineFollowStrategy lStrategy = determineStrategy();
231
          if(lStrategy == LineFollowStrategy::DOCKING){
232
            utState = states::OCCUPY;
233
            break;
234
          }
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){
235 215
          lf.setStrategy(lStrategy);
236
          lf.followLine(rpmSpeed);
237
          setRpmSpeed(rpmSpeed);
238
          break;
239 216
        }
240
        case states::OCCUPY:{
241
          // Get Wheel proxy sensors
242
          int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
243
          int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
244
          // Check for black
245
          if ((WL+WR) < proxyWheelThresh){ /*TODO: Only check for one sensor */
246
            global.motorcontrol.setTargetRPM(0,0);
247 217

  
248
            // Stop when in docking state and take further actions
249
            if(lf.getStrategy() == LineFollowStrategy::DOCKING){
250
              if(checkDockingSuccess(this)){
251
                utState = states::CHARGING;
252
                break;
253
              }else{
254
                correctPosition(this, lf, 250);
255
                lf.setStrategy(LineFollowStrategy::DOCKING);
256
                // break;
257
              }
258
            }else{
259
              checkForMotion(this);
260
              // 180° Rotation 
261
              global.distcontrol.setTargetPosition(0, 3141592, 10000);
262
              // BaseThread::sleep(8000);
263
              checkForMotion(this);
264
              correctPosition(this, lf, 250);
265
              // break;
266
              lf.setStrategy(LineFollowStrategy::DOCKING);
267
              
268
            }
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++;
269 226
          }
270

  
271
          // Set correct following
272
          // lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
273
          lf.followLine(rpmSpeed);
227
        }else{
228
          whiteBuf = 0;
274 229
          setRpmSpeed(rpmSpeed);
275
          
276
          break;
277 230
        }
278
        case states::RELEASE:{
279
          break;
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;
280 239
        }
281
        case states::CHARGING:{
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){
282 248
          setRpmSpeed(stop);
283
          break;
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;
284 255
        }
285
        default:{
286
          break;
256
        break;
257
      // ---------------------------------------
258
      case states::CORRECT_POSITIONING:
259
        if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){
260
          lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
287 261
        }
288
      }
262
        lf.followLine(rpmSpeed);
263
        setRpmSpeed(rpmSpeed);
289 264

  
290
    }else{
291
      // Stop
292
        setRpmSpeed(stop);
293
      }
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);
294 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;
295 334
    this->sleep(CAN::UPDATE_PERIOD);
296 335
  }
297 336

  
devices/DiWheelDrive/userthread.hpp
3 3

  
4 4
#include <ch.hpp>
5 5
#include <amiroosconf.h>
6
#include <amiro/Color.h>
7
// #include "global.hpp"
8

  
9
// Speed when driving towards the docking station
10
#define CHARGING_SPEED 5
11
// Thresh to determain how much update steps should pass while alining
12
#define MAX_CORRECTION_STEPS 250
13
// Thresh for wheel proxy sensors, when summed values fall below the state changes
14
#define PROXY_WHEEL_THRESH 18000
15
// Thresh for how long (update steps) the front sensors are allowed to detect white
16
#define WHITE_COUNT_THRESH 120
17
// Rotation around 180 degrees in microradian
18
#define ROTATION_180 3141592
19
// Rotation around -20 degrees in microradian
20
#define ROTATION_20 -349065
21
#define ROTATION_DURATION 10000
22

  
6 23

  
7 24
namespace amiro {
8 25

  
26

  
9 27
class UserThread : public chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>
10 28
{
11
public:
29

  
30
  // Messages which can be received and trigger state changes
31
  public:
32
    enum msg_content{
33
      STOP, 
34
      START,
35
      EDGE_LEFT,
36
      EDGE_RIGHT,
37
      FUZZY,
38
      DOCK,
39
      UNDOCK,
40
      CHARGE
41
    };
42

  
43
    // States of user thread state machine
44
    enum states{
45
      FOLLOW_LINE,
46
      RELEASE,
47
      CHARGING,
48
      DETECT_STATION,
49
      CORRECT_POSITIONING,
50
      REVERSE,
51
      CHECK_POSITIONING,
52
      IDLE
53
    };
54

  
55

  
56

  
12 57
  explicit UserThread();
13 58

  
14 59
  virtual ~UserThread();
15 60

  
16 61
  virtual msg_t main();
62

  
63
private:
64
  void setRpmSpeed(const int (&rpmSpeed)[2]);
65
  void lightOneLed(Color color, int idx);
66
  void lightAllLeds(Color color);
67
  /**
68
   * Uses light ring indicate the state of charge.
69
   */
70
  void showChargingState();
71
  void checkForMotion();
72
  bool checkPinVoltage();
73
  bool checkPinEnabled();
74

  
75
  /**
76
   * Check if current position changes when the wheel are deactivated.
77
   * 
78
   * When AMiRo drives towards the loading station, it stops when a specific marker is reached.
79
   * In order to validate that the AMiRo is correctly positioned in the loading station
80
   * the wheels are turned off. When the position remains the same the docking procedure
81
   * was successful (return 1) otherwise a correction is needed (return 0). 
82
   */
83
  int checkDockingSuccess();
17 84
};
18 85

  
19 86
} // end of namespace amiro

Also available in: Unified diff