amiro-os / devices / DiWheelDrive / global.hpp @ e5606def
History | View | Annotate | Download (9 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] = {30,30}; |
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 |
uint32_t stateTracker[24] = { 0 }; |
173 |
uint32_t stateTransitionCounter = 0;
|
174 |
// Line Following thresholds set due to calibration
|
175 |
// MaxDelta: 18676, FL: 4289, FR: 22965
|
176 |
// Thresh FL: 5241, FR: 25528
|
177 |
// Good for amiro 25
|
178 |
struct line_pid {
|
179 |
int K_p = 0.0f; |
180 |
float K_i = 0.0f; |
181 |
float K_d = 0.0f; |
182 |
int threshProxyL = 5241; |
183 |
int threshProxyR = 25528; |
184 |
int BThresh = 5; |
185 |
int WThresh = 25528; |
186 |
}; |
187 |
|
188 |
line_pid linePID; |
189 |
|
190 |
|
191 |
int threshWhite = 40000; |
192 |
|
193 |
// PID for line following:
|
194 |
|
195 |
|
196 |
// Integral part
|
197 |
int accumHist = 0; |
198 |
int oldError = 0; |
199 |
|
200 |
// Struct for saving motor gains
|
201 |
|
202 |
int resetProtect = 1; |
203 |
motorGains motorConfigGains; |
204 |
motorGains stopGains; |
205 |
|
206 |
types::position startPos; |
207 |
types::position endPos; |
208 |
|
209 |
// Line Following strategy
|
210 |
// 0 EDGE_RIGHT
|
211 |
// 1 EDGE_LEFT
|
212 |
// 2 FUZZY
|
213 |
// 3 DOCKING
|
214 |
int lfStrategy = 3; |
215 |
|
216 |
// CAN communication line:
|
217 |
// Will be set to true when message was received
|
218 |
bool msgReceived = false; |
219 |
|
220 |
// bool chargingRequest = false;
|
221 |
// uint8_t charge = 0;
|
222 |
// uint8_t actualCharge = 0;
|
223 |
|
224 |
// Debugging stuff --------------
|
225 |
int forwardSpeed = 5; |
226 |
int enableRecord = 0; |
227 |
|
228 |
bool triggerCan = false; |
229 |
uint8_t strategyTest = 0;
|
230 |
// CANTxFrame powerFrame;
|
231 |
|
232 |
|
233 |
// Buffer for sensor values
|
234 |
struct sensorRecord
|
235 |
{ |
236 |
int FL = 0; |
237 |
int FR = 0; |
238 |
int delta = 0; |
239 |
int error = 0; |
240 |
}; |
241 |
|
242 |
int sensSamples = 0; |
243 |
sensorRecord senseRec[1];
|
244 |
sensorRecord maxDist; |
245 |
|
246 |
// TODO: Move defines to a more suiting place
|
247 |
// Test cases
|
248 |
#define TEST_CASES 10 |
249 |
/* Test Results */
|
250 |
bool testres[TEST_CASES];
|
251 |
// Max amount of nodes in a map
|
252 |
#define MAX_NODES 10 |
253 |
// Amount of attributes that can be passed to each node
|
254 |
#define NODE_ATTRIBUTES 3 |
255 |
|
256 |
/* TODO gloal test map could be also used for later map implementation */
|
257 |
/* AmiroMap already knows about the global object such that the map does not need */
|
258 |
/* to be passed explicitly */
|
259 |
|
260 |
// Global test map
|
261 |
uint8_t testmap[MAX_NODES][NODE_ATTRIBUTES]; |
262 |
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 |
{ |
295 |
return;
|
296 |
} |
297 |
|
298 |
~Global() |
299 |
{ |
300 |
return;
|
301 |
} |
302 |
}; |
303 |
|
304 |
#endif /* AMIRO_GLOBAL_HPP_ */ |