amiro-os / devices / DiWheelDrive / global.hpp @ 1b3adcdd
History | View | Annotate | Download (7.71 KB)
1 | 58fe0e0b | Thomas Schöpping | #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 | b4885314 | Thomas Schöpping | /* proximity rate */ VCNL4020::PROX_RATE_125
|
59 | 58fe0e0b | Thomas Schöpping | }; |
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 | 2330e415 | Georg Alberding | int rpmForward[2] = {20,20}; |
168 | int rpmSoftLeft[2] = {10,20}; |
||
169 | int rpmHardLeft[2] = {5,20}; |
||
170 | c76baf23 | Georg Alberding | int rpmSoftRight[2] = {rpmSoftLeft[1],rpmSoftLeft[0]}; |
171 | int rpmHardRight[2] = {rpmHardLeft[1],rpmHardLeft[0]}; |
||
172 | 22b85da1 | galberding | |
173 | 58fe0e0b | Thomas Schöpping | |
174 | 12463563 | galberding | // Line Following thresholds set due to calibration
|
175 | 88afb834 | galberding | // MaxDelta: 18676, FL: 4289, FR: 22965
|
176 | 22b85da1 | galberding | // Thresh FL: 5241, FR: 25528
|
177 | int threshProxyL = 5241; |
||
178 | int threshProxyR = 25528; |
||
179 | 1b3adcdd | galberding | int threshWhite = 51056; |
180 | 22b85da1 | galberding | |
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 | 1b3adcdd | galberding | int enableRecord = 1; |
193 | 88afb834 | galberding | |
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 | 22b85da1 | galberding | sensorRecord maxDist; |
206 | 88afb834 | galberding | |
207 | 22b85da1 | galberding | // -----------------------------
|
208 | 58fe0e0b | Thomas Schöpping | 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 | b4885314 | Thomas Schöpping | odometry(&increments, &l3g4200d), |
234 | 58fe0e0b | Thomas Schöpping | 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_ */ |