Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / global.hpp @ 1b3adcdd

History | View | Annotate | Download (7.71 KB)

1
#ifndef AMIRO_GLOBAL_HPP_
2
#define AMIRO_GLOBAL_HPP_
3

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

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

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

    
31
using namespace amiro;
32

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

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

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

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

    
77
  PCA9544<true> HW_PCA9544;
78

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

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

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

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

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

    
104
  HWSPIDriver HW_SPI1_ACCEL;
105
  HWSPIDriver HW_SPI1_GYRO;
106

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

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

    
151
  INA219::Driver ina219;
152

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

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

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

    
164
  DiWheelDrive robot;
165

    
166
  UserThread userThread;
167
  int rpmForward[2] = {20,20};
168
  int rpmSoftLeft[2] = {10,20};
169
  int rpmHardLeft[2] = {5,20};
170
  int rpmSoftRight[2] = {rpmSoftLeft[1],rpmSoftLeft[0]};
171
  int rpmHardRight[2] = {rpmHardLeft[1],rpmHardLeft[0]};
172
  
173

    
174
  // Line Following thresholds set due to calibration
175
  // MaxDelta: 18676, FL: 4289, FR: 22965
176
  // Thresh FL: 5241, FR: 25528
177
  int threshProxyL = 5241;
178
  int threshProxyR = 25528;
179
  int threshWhite = 51056;
180

    
181
  // PID for line following:
182
  float K_p = 0.0f;
183
  float K_i = 0.0f;
184
  float K_d = 0.0f;
185

    
186
  // Integral part
187
  int accumHist = 0;
188
  int oldError = 0;
189

    
190
// Debugging stuff --------------
191
  int forwardSpeed = 10;
192
  int enableRecord = 1;
193

    
194
  // Buffer for sensor values
195
  struct sensorRecord
196
  {
197
    int FL = 0;
198
    int FR = 0;
199
    int delta = 0;
200
    int error = 0;
201
  };
202
  
203
  int sensSamples = 0;
204
  sensorRecord senseRec[1000];
205
  sensorRecord maxDist;
206
  
207
// -----------------------------
208
public:
209
  Global() :
210
    HW_I2C1(&I2CD1), HW_I2C2(&I2CD2),
211
    HW_PCA9544(&HW_I2C2, 0x07u),
212
    V_I2C2{{VI2CDriver(&HW_PCA9544, 0),
213
            VI2CDriver(&HW_PCA9544, 1),
214
            VI2CDriver(&HW_PCA9544, 2),
215
            VI2CDriver(&HW_PCA9544, 3)}
216
          },
217
    vcnl4020{{/* front left  */ VCNL4020(&V_I2C2[0], &vcnl4020_config),
218
              /* left wheel  */ VCNL4020(&V_I2C2[1], &vcnl4020_config),
219
              /* right wheel */ VCNL4020(&V_I2C2[2], &vcnl4020_config),
220
              /* front right */ VCNL4020(&V_I2C2[3], &vcnl4020_config)}
221
            },
222
    hmc5883l(&HW_I2C1, &hmc5883l_config),
223
    HW_SPI1_ACCEL(&SPID1, &accel_spi1_config), HW_SPI1_GYRO(&SPID1, &gyro_spi1_config),
224
    lis331dlh(&HW_SPI1_ACCEL),
225
    l3g4200d(&HW_SPI1_GYRO),
226
    ina219(HW_I2C2, 0x40u),
227
    ltc4412(),
228
    at24c01(0x400u / 0x08u, 0x08u, 500u, &HW_I2C2),
229
    memory(at24c01, /*BMSV*/ 1, /*bmsv*/ 3, /*HMV*/ 1, /*hmv*/ 0), // bmsv changed von 2 auf 3
230
    increments(&QEID3, &QEID4),
231
    motorcontrol(&PWMD2, &increments, GPIOB, GPIOB_POWER_EN, &memory),
232
    distcontrol(&motorcontrol, &increments),
233
    odometry(&increments, &l3g4200d),
234
    sercanmux1(&SD1, &CAND1, CAN::DI_WHEEL_DRIVE_ID),
235
    robot(&CAND1),
236
    userThread()
237
  {
238
    return;
239
  }
240

    
241
  ~Global()
242
  {
243
    return;
244
  }
245
};
246

    
247
#endif /* AMIRO_GLOBAL_HPP_ */