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_ */ |