Revision 019224ff

View differences:

devices/DiWheelDrive/userthread.cpp
130 130
   int whiteBuf = 0;
131 131
   int proxyBuf = 0;
132 132
   int correctionStep = 0;
133
   uint16_t rProx[8]; // buffer for ring proxy values
133 134
  int rpmSpeed[2] = {0};
134 135
  int stop[2] = {0};
135 136
  LineFollowStrategy lStrategy = LineFollowStrategy::FUZZY;
......
199 200
    newState = utState;
200 201

  
201 202
    // Get sensor data 
202
    int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
203
    int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
203
    uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
204
    uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
205
    for(int i=0; i<8;i++){
206
      rProx[i] = global.robot.getProximityRingValue(i);
207
    }
204 208
    // int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
205 209
    // int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
206 210
    switch(utState){
......
254 258
        break;
255 259
      // ---------------------------------------
256 260
      case states::DETECT_STATION:
257
        if (global.forwardSpeed != CHARGING_SPEED){
258
          global.forwardSpeed = CHARGING_SPEED;
259
        }
261
        // if (global.forwardSpeed != CHARGING_SPEED){
262
        //   global.forwardSpeed = CHARGING_SPEED;
263
        // }
260 264
        if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
261 265
          lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
262 266
        }
......
264 268
        lf.followLine(rpmSpeed);
265 269
        setRpmSpeed(rpmSpeed);
266 270
        // // Detect marker before docking station
267
        if ((WL+WR) < PROXY_WHEEL_THRESH){
271
        // if ((WL+WR) < PROXY_WHEEL_THRESH){
272
        // Use proxy ring 
273
        if ((rProx[3]+rProx[4]) > RING_PROX_FRONT_THRESH){
274

  
268 275
          setRpmSpeed(stop);
269 276
          checkForMotion();
270 277
          // 180° Rotation 
......
276 283
        break;
277 284
      // ---------------------------------------
278 285
      case states::CORRECT_POSITIONING:
286
        if (global.forwardSpeed != CHARGING_SPEED){
287
          global.forwardSpeed = CHARGING_SPEED;
288
        }
279 289
        if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){
280 290
          lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
281 291
        }
......
298 308
        lf.followLine(rpmSpeed);
299 309
        setRpmSpeed(rpmSpeed);
300 310

  
301
        if ((WL+WR) < PROXY_WHEEL_THRESH){
311
        // if ((WL+WR) < PROXY_WHEEL_THRESH){
312
        // Is of those sensors at it max val means that the AMiRo cant drive back
313
        // so check if correctly positioned
314
        //definitely wrong positioned -> correct position directly without rotation
315
        if ((rProx[0] >= PROX_MAX_VAL && rProx[7] < PROX_MAX_VAL) || (rProx[0] < PROX_MAX_VAL && rProx[7] >= PROX_MAX_VAL)){
316
          setRpmSpeed(stop);
317
          checkForMotion();
318
          newState = states::CORRECT_POSITIONING;
319
        }
320
        // Both sensors are at a wall -> potential loading station dock and on magnet -> need to rotate
321
        if((rProx[0] >= PROX_MAX_VAL) || (rProx[7] >= PROX_MAX_VAL)){
302 322
          setRpmSpeed(stop);
303 323
          checkForMotion();
304 324
          newState = states::CHECK_POSITIONING;
......
324 344
            // No voltage on pins -> falsely docked
325 345
            // deactivate pins
326 346
            global.robot.requestCharging(0);
327
            newState = states::CORRECT_POSITIONING;
347
            newState = states::RELEASE_TO_CORRECT;
328 348
          }
329 349
        }
350
        break;
351
      // ---------------------------------------
352
      case states::RELEASE_TO_CORRECT:
353
        global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
354
        checkForMotion();
355
        // move 1cm forward
356
        global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
357
        checkForMotion();
358
        // rotate back
359
        global.distcontrol.setTargetPosition(0, -2*ROTATION_20, ROTATION_DURATION);
360
        checkForMotion();
361

  
362
        global.distcontrol.setTargetPosition(7000, 0, ROTATION_DURATION);
363
        checkForMotion();
364
        newState = states::CORRECT_POSITIONING;
365
        break;
330 366
      // ---------------------------------------
331 367
      case states::CHARGING:
332 368
        if (global.motorcontrol.getMotorEnable()){
......
355 391
          }
356 392
          //Rotate -20° to free from magnet
357 393
          global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
358
          checkForMotion();
394
        checkForMotion();
395
        // move 1cm forward
396
        global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
397
        checkForMotion();
398
        // rotate back
399
        global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
400
        checkForMotion();
401

  
402
        global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
403
        checkForMotion();
359 404
          lStrategy = LineFollowStrategy::EDGE_RIGHT;
360 405
          newState = states::FOLLOW_LINE;
406
          whiteBuf = -100;
361 407
        }
362 408
        lightAllLeds(Color::BLACK);
363 409
        break;
devices/DiWheelDrive/userthread.hpp
9 9
// Speed when driving towards the docking station
10 10
#define CHARGING_SPEED 5
11 11
// Thresh to determain how much update steps should pass while alining
12
#define MAX_CORRECTION_STEPS 250
12
#define MAX_CORRECTION_STEPS 260
13 13
// Thresh for wheel proxy sensors, when summed values fall below the state changes
14 14
#define PROXY_WHEEL_THRESH 18000
15 15
// Thresh for detecting obsticles
......
22 22
#define ROTATION_20 -349065
23 23
#define ROTATION_DURATION 10000
24 24

  
25
#define RING_PROX_FRONT_THRESH 20000
26
#define PROX_MAX_VAL 65530
25 27

  
26 28
namespace amiro {
27 29

  
......
46 48
    enum states{
47 49
      FOLLOW_LINE,
48 50
      RELEASE,
51
      RELEASE_TO_CORRECT,
49 52
      CHARGING,
50 53
      DETECT_STATION,
51 54
      CORRECT_POSITIONING,

Also available in: Unified diff