Revision f1d13b04

View differences:

devices/DiWheelDrive/amiro_map.cpp
247 247
uint8_t AmiroMap::getNearest(types::position *p1) {
248 248

  
249 249
  uint8_t actualStrategy = this->lfStrategy == EDGE_LEFT ? 1 : 2;
250
  uint32_t thresh = 1;          // TODO: find good thresh value in cm
250
  uint32_t thresh = global->nodeDistThresh;          // TODO: find good thresh value in cm
251 251
  uint8_t id = 255;
252 252
  uint32_t smallestDist = thresh;
253 253
  uint8_t currentStrategy;
......
307 307
  nodeList[id].visited |= state.strategy;
308 308
  return id;
309 309
}
310

  
311
void AmiroMap::reset(){
312
  this->nodeCount = 0;
313
  this->state.current = 0;
314
  this->state.next= 0;
315

  
316
}
devices/DiWheelDrive/amiro_map.hpp
29 29
    } p;
30 30
  };
31 31

  
32
  struct map_state {
33
    // 0 - left, 1- right
34
    uint8_t strategy;
35
    // Node ID of last detected fixpoint
36
    uint8_t current;
37
    // Next node ID
38
    uint8_t next;
39
    //Traveled Distance between current and next in %
40
    uint32_t dist;
41
    // True if the current loaded map is valid
42
    bool valid;
43
    // Length of the currently traveled edge
44
    uint32_t eLength;
45
  };
46 32

  
47 33
class AmiroMap {
48 34
public:
......
143 129

  
144 130
  // Either create new fxpoint or assign point to existing one
145 131
  uint8_t assignFxp(types::position *p1);
132
  void reset();
146 133
};
147 134

  
148 135
}; // namespace amiro
devices/DiWheelDrive/global.hpp
1 1
#ifndef AMIRO_GLOBAL_HPP_
2 2
#define AMIRO_GLOBAL_HPP_
3 3

  
4
#include <cstdint>
4 5
#include <hal.h>
5 6
#include <qei.h>
6 7

  
......
260 261
  // Global test map
261 262
  uint8_t testmap[MAX_NODES][NODE_ATTRIBUTES];
262 263
  uint8_t tcase = 0;
263
  // -----------------------------
264

  
265
public:
266
  Global() :
267
    HW_I2C1(&I2CD1), HW_I2C2(&I2CD2),
268
    HW_PCA9544(&HW_I2C2, 0x07u),
269
    V_I2C2{{VI2CDriver(&HW_PCA9544, 0),
270
            VI2CDriver(&HW_PCA9544, 1),
271
            VI2CDriver(&HW_PCA9544, 2),
272
            VI2CDriver(&HW_PCA9544, 3)}
273
          },
274
    vcnl4020{{/* front left  */ VCNL4020(&V_I2C2[0], &vcnl4020_config),
275
              /* left wheel  */ VCNL4020(&V_I2C2[1], &vcnl4020_config),
276
              /* right wheel */ VCNL4020(&V_I2C2[2], &vcnl4020_config),
277
              /* front right */ VCNL4020(&V_I2C2[3], &vcnl4020_config)}
278
            },
279
    hmc5883l(&HW_I2C1, &hmc5883l_config),
280
    HW_SPI1_ACCEL(&SPID1, &accel_spi1_config), HW_SPI1_GYRO(&SPID1, &gyro_spi1_config),
281
    lis331dlh(&HW_SPI1_ACCEL),
282
    l3g4200d(&HW_SPI1_GYRO),
283
    ina219(HW_I2C2, 0x40u),
284
    ltc4412(),
285
    at24c01(0x400u / 0x08u, 0x08u, 500u, &HW_I2C2),
286
    memory(at24c01, /*BMSV*/ 1, /*bmsv*/ 3, /*HMV*/ 1, /*hmv*/ 0), // bmsv changed von 2 auf 3
287
    increments(&QEID3, &QEID4),
288
    motorcontrol(&PWMD2, &increments, GPIOB, GPIOB_POWER_EN, &memory),
289
    distcontrol(&motorcontrol, &increments),
290
    odometry(&increments, &l3g4200d),
291
    sercanmux1(&SD1, &CAND1, CAN::DI_WHEEL_DRIVE_ID),
292
    robot(&CAND1),
293
    userThread()
294
  {
264
  uint32_t nodeDistThresh = 1;
265
      // -----------------------------
266

  
267
      public : Global()
268
      : HW_I2C1(&I2CD1),
269
        HW_I2C2(&I2CD2),
270
        HW_PCA9544(&HW_I2C2, 0x07u),
271
        V_I2C2{{VI2CDriver(&HW_PCA9544, 0), VI2CDriver(&HW_PCA9544, 1),
272
                VI2CDriver(&HW_PCA9544, 2), VI2CDriver(&HW_PCA9544, 3)}},
273
        vcnl4020{{/* front left  */ VCNL4020(&V_I2C2[0], &vcnl4020_config),
274
                  /* left wheel  */ VCNL4020(&V_I2C2[1], &vcnl4020_config),
275
                  /* right wheel */ VCNL4020(&V_I2C2[2], &vcnl4020_config),
276
                  /* front right */ VCNL4020(&V_I2C2[3], &vcnl4020_config)}},
277
        hmc5883l(&HW_I2C1, &hmc5883l_config),
278
        HW_SPI1_ACCEL(&SPID1, &accel_spi1_config),
279
        HW_SPI1_GYRO(&SPID1, &gyro_spi1_config),
280
        lis331dlh(&HW_SPI1_ACCEL),
281
        l3g4200d(&HW_SPI1_GYRO),
282
        ina219(HW_I2C2, 0x40u),
283
        ltc4412(),
284
        at24c01(0x400u / 0x08u, 0x08u, 500u, &HW_I2C2),
285
        memory(at24c01, /*BMSV*/ 1, /*bmsv*/ 3, /*HMV*/ 1,
286
               /*hmv*/ 0), // bmsv changed von 2 auf 3
287
        increments(&QEID3, &QEID4),
288
        motorcontrol(&PWMD2, &increments, GPIOB, GPIOB_POWER_EN, &memory),
289
        distcontrol(&motorcontrol, &increments),
290
        odometry(&increments, &l3g4200d),
291
        sercanmux1(&SD1, &CAND1, CAN::DI_WHEEL_DRIVE_ID),
292
        robot(&CAND1),
293
        userThread() {
295 294
    return;
296 295
  }
297 296

  

Also available in: Unified diff