Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / global.hpp @ 4d54a507

History | View | Annotate | Download (9.13 KB)

1
#ifndef AMIRO_GLOBAL_HPP_
2
#define AMIRO_GLOBAL_HPP_
3

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

    
8
#include <stdlib.h>
9
#include <array>
10

    
11
#include <board.h>
12
#include <amiro/proximity/vcnl4020.hpp>
13
#include <amiro/bus/i2c/HWI2CDriver.hpp>
14
#include <amiro/bus/i2c/mux/pca9544.hpp>
15
#include <amiro/bus/i2c/VI2CDriver.hpp>
16
#include <amiro/magneto/hmc5883l.hpp>
17
#include <amiro/bus/spi/HWSPIDriver.hpp>
18
#include <amiro/accel/lis331dlh.hpp>
19
#include <amiro/gyro/l3g4200d.hpp>
20
#include <amiro/power/ina219.hpp>
21
#include <amiro/power/ltc4412.hpp>
22
#include <amiro/eeprom/at24.hpp>
23
#include <amiro/FileSystemInputOutput/FSIODiWheelDrive.hpp>
24
#include <amiro/serial_reset/serial_can_mux.hpp>
25
#include <amiro/MotorIncrements.h>
26
#include <amiro/MotorControl.h>
27
#include <amiro/DistControl.h>
28
#include <amiro/Odometry.h>
29
#include <DiWheelDrive.h>
30
#include <userthread.hpp>
31

    
32
using namespace amiro;
33

    
34
class Global final
35
{
36
public:
37
  SerialConfig sd1_config{
38
    /* speed        */ 115200,
39
    /* CR1 register */ 0,
40
    /* CR2 register */ 0,
41
    /* CR3 register */ 0
42
  };
43

    
44
  I2CConfig i2c1_config{
45
    /* I2C mode                 */ OPMODE_I2C,
46
    /* frequency                */ 400000,
47
    /* I2C fast mode duty cycle */ FAST_DUTY_CYCLE_2
48
  };
49
  I2CConfig i2c2_config{
50
    /* I2C mode                 */ OPMODE_I2C,
51
    /* frequency                */ 400000,
52
    /* I2C fast mode duty cycle */ FAST_DUTY_CYCLE_2
53
  };
54

    
55
  VCNL4020::VCNL4020Config vcnl4020_config{
56
    /* command             */ VCNL4020::ALS_EN | VCNL4020::PROX_EN | VCNL4020::SELFTIMED_EN,
57
    /* ambient parameter   */ VCNL4020::AMBIENT_RATE_2 | VCNL4020::AMBIENT_AUTO_OFFSET | VCNL4020::AMBIENT_AVG_32,
58
    /* IR LED current [mA] */ 200u,
59
    /* proximity rate      */ VCNL4020::PROX_RATE_125
60
  };
61

    
62
  /**
63
   * @brief I2C Bus 1
64
   * Conected devices:
65
   *   HMC5883L
66
   */
67
  HWI2CDriver HW_I2C1;
68
  /**
69
   * @brief I2C Bus 2
70
   * Connected devices:
71
   *   PCA9544
72
   *   VCNL4020
73
   *   INA219
74
   *   AT24Cxx
75
   */
76
  HWI2CDriver HW_I2C2;
77

    
78
  PCA9544<true> HW_PCA9544;
79

    
80
  std::array<VI2CDriver, 4> V_I2C2;
81

    
82
  std::array<VCNL4020, 4> vcnl4020;
83

    
84
  HMC5883L::HMC5883LConfig hmc5883l_config{
85
    /* ctrlA */ HMC5883L::DO_20_HZ | HMC5883L::MS_NORMAL | HMC5883L::MA_AVG8,
86
    /* ctrlB */ HMC5883L::GN_5_GA,
87
    /* mode  */ HMC5883L::MD_CONTCV | HMC5883L::HS_DISABLE
88
  };
89
  HMC5883L hmc5883l;
90

    
91
  SPIConfig accel_spi1_config{
92
    /* callback function pointer   */ NULL,
93
    /* chip select line port       */ GPIOC,
94
    /* chip select line pad number */ GPIOC_ACCEL_SS_N,
95
    /* initialzation data          */ SPI_CR1_BR_0
96
  };
97

    
98
  SPIConfig gyro_spi1_config{
99
    /* callback function pointer   */ NULL,
100
    /* chip select line port       */ GPIOC,
101
    /* chip select line pad number */ GPIOC_GYRO_SS_N,
102
    /* initialzation data          */ SPI_CR1_BR_0
103
  };
104

    
105
  HWSPIDriver HW_SPI1_ACCEL;
106
  HWSPIDriver HW_SPI1_GYRO;
107

    
108
  LIS331DLH::LIS331DLHConfig accel_run_config{
109
    /* ctrl1                     */ LIS331DLH::PM_ODR | LIS331DLH::DR_50HZ_37LP | LIS331DLH::ZEN | LIS331DLH::YEN | LIS331DLH::XEN,
110
    /* ctrl2                     */ 0x00u,
111
    /* ctrl3                     */ LIS331DLH::INT_LOW | LIS331DLH::I1_CFG_DRY,
112
    /* ctrl4                     */ LIS331DLH::BDU_CONT | LIS331DLH::BLE_LE | LIS331DLH::FS_4G | LIS331DLH::SIM_4WI,
113
    /* ctrl5                     */ LIS331DLH::SLEEP_TO_WAKE_OFF,
114
    /* interrupt 1 configuration */ {/* configuration */ LIS331DLH::AOI_OR_INT,
115
                                     /* ths           */ 0x00u,
116
                                     /* duration      */ 0x00u},
117
    /* interrupt 2 configuration */ {/* configuration */ LIS331DLH::AOI_OR_INT,
118
                                     /* ths           */ 0x00u,
119
                                     /* duration      */ 0x00u}
120
  };
121
  LIS331DLH::LIS331DLHConfig accel_sleep_config{
122
    /* ctrl1                     */ LIS331DLH::PM_0_5_HZ | LIS331DLH::DR_50HZ_37LP | LIS331DLH::XEN,
123
    /* ctrl2                     */ 0x00u,
124
    /* ctrl3                     */ LIS331DLH::INT_LOW | LIS331DLH::I1_CFG_I1,
125
    /* ctrl4                     */ LIS331DLH::BDU_CONT | LIS331DLH::BLE_LE | LIS331DLH::FS_4G | LIS331DLH::SIM_4WI,
126
    /* ctrl5                     */ LIS331DLH::SLEEP_TO_WAKE_ON,
127
    /* interrupt 1 configuration */ {/* configuration */ LIS331DLH::AOI_OR_INT | LIS331DLH::XHIE,
128
                                     /* ths           */ 0x10u,
129
                                     /* duration      */ 0x02u},
130
    /* interrupt 2 configuration */ {/* configuration */ LIS331DLH::AOI_OR_INT,
131
                                     /* ths           */ 0x00u,
132
                                     /* duration      */ 0x00u}
133
  };
134
  LIS331DLH lis331dlh;
135

    
136
  L3G4200D::L3G4200DConfig gyro_run_config{
137
    /* ctrl reg 1 */ L3G4200D::DR_100_HZ | L3G4200D::BW_25 | L3G4200D::PD | L3G4200D::ZEN | L3G4200D::YEN | L3G4200D::XEN,
138
    /* ctrl reg 2 */ 0,
139
    /* ctrl reg 3 */ L3G4200D::I2_DRDY,
140
    /* ctrl reg 4 */ L3G4200D::FS_500_DPS | L3G4200D::SIM_4W,
141
    /* ctrl reg 5 */ 0
142
  };
143
  L3G4200D::L3G4200DConfig gyro_sleep_config{
144
    /* ctrl reg 1 */ L3G4200D::DR_100_HZ | L3G4200D::BW_25 | L3G4200D::PD,
145
    /* ctrl reg 2 */ gyro_run_config.ctrl2,
146
    /* ctrl reg 3 */ gyro_run_config.ctrl3,
147
    /* ctrl reg 4 */ gyro_run_config.ctrl4,
148
    /* ctrl reg 5 */ gyro_run_config.ctrl5
149
  };
150
  L3G4200D l3g4200d;
151

    
152
  INA219::Driver ina219;
153

    
154
  LTC4412wEN<(uintptr_t)GPIOC, GPIOC_PATH_DCSTAT, (uintptr_t)GPIOC, GPIOC_PATH_DCEN> ltc4412;
155

    
156
  AT24 at24c01;
157
  fileSystemIo::FSIODiWheelDrive memory;
158

    
159
  MotorIncrements increments;
160
  MotorControl motorcontrol;
161
  DistControl distcontrol;
162
  Odometry odometry;
163
  SerialCanMux sercanmux1;
164

    
165
  DiWheelDrive robot;
166

    
167
  UserThread userThread;
168
  int rpmForward[2] = {30,30};
169
  int rpmSoftLeft[2] = {10,20};
170
  int rpmHardLeft[2] = {5,20};
171
  int rpmSoftRight[2] = {rpmSoftLeft[1],rpmSoftLeft[0]};
172
  int rpmHardRight[2] = {rpmHardLeft[1],rpmHardLeft[0]};
173
  uint32_t stateTracker[24] = { 0 };
174
  uint32_t stateTransitionCounter = 0;
175
  // Line Following thresholds set due to calibration
176
  // MaxDelta: 18676, FL: 4289, FR: 22965
177
  // Thresh FL: 5241, FR: 25528
178
  // Good for amiro 25
179
  struct line_pid {
180
    int K_p = 0.0f;
181
    float K_i = 0.0f;
182
    float K_d = 0.0f;
183
    int threshProxyL = 5241;
184
    int threshProxyR = 25528;
185
    int BThresh = 5;
186
    int WThresh = 25528;
187
  };
188

    
189
  line_pid linePID;
190

    
191

    
192
  int threshWhite = 40000;
193

    
194
  // PID for line following:
195

    
196

    
197
  // Integral part
198
  int accumHist = 0;
199
  int oldError = 0;
200

    
201
// Struct for saving motor gains
202

    
203
  int resetProtect = 1;
204
  motorGains motorConfigGains;
205
  motorGains stopGains;
206

    
207
  types::position startPos;
208
  types::position endPos;
209

    
210
  // Line Following strategy
211
  // 0 EDGE_RIGHT
212
  // 1 EDGE_LEFT
213
  // 2 FUZZY
214
  // 3 DOCKING
215
  int lfStrategy = 3;
216

    
217
  // CAN communication line:
218
  // Will be set to true when message was received
219
  bool msgReceived = false;
220

    
221
  // bool chargingRequest = false;
222
  // uint8_t charge = 0;
223
  // uint8_t actualCharge = 0;
224

    
225
// Debugging stuff --------------
226
  int forwardSpeed = 5;
227
  int enableRecord = 0;
228

    
229
  bool triggerCan = false;
230
  uint8_t strategyTest = 0;
231
  // CANTxFrame powerFrame;
232

    
233

    
234
  // Buffer for sensor values
235
  struct sensorRecord
236
  {
237
    int FL = 0;
238
    int FR = 0;
239
    int delta = 0;
240
    int error = 0;
241
  };
242

    
243
  int sensSamples = 0;
244
  sensorRecord senseRec[1];
245
  sensorRecord maxDist;
246

    
247
  // TODO: Move defines to a more suiting place
248
  // Test cases
249
#define TEST_CASES 10
250
  /* Test Results */
251
  bool testres[TEST_CASES];
252
  // Max amount of nodes in a map
253
#define MAX_NODES 10
254
  // Amount of attributes that can be passed to each node
255
#define NODE_ATTRIBUTES 3
256

    
257
  /* TODO gloal test map could be also used for later map implementation */
258
  /* AmiroMap already knows about the global object such that the map does not need */
259
  /* to be passed explicitly */
260

    
261
  // Global test map
262
  uint8_t testmap[MAX_NODES][NODE_ATTRIBUTES];
263
  uint8_t tcase = 0;
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() {
294
    return;
295
  }
296

    
297
  ~Global()
298
  {
299
    return;
300
  }
301
};
302

    
303
#endif /* AMIRO_GLOBAL_HPP_ */