SBB Micro
Source code for the self-balancing-bike microcontroller unit (Teensy 4.1-based). 🚀️
Loading...
Searching...
No Matches
objects.h
Go to the documentation of this file.
1
2#ifndef __OBJECTS_H__
3#define __OBJECTS_H__
4
10
18
19//namespaces
30namespace LedMode {
31 constexpr int8_t DEF = 0;
32 constexpr int8_t LOG = 1;
33 constexpr int8_t MTP = 2;
34 constexpr int8_t ERR = 3;
35 constexpr int8_t MTPWAIT = 4;
36}
37
38//strcut def
50struct ImuData { //raw acc (g), gyro (mdeg/s), and mag (mG) data
51 int32_t accs_mg[3];
52 int32_t gyros_mdps[3];
53 int32_t mags_mG[3];
54 float gyros_offset[3];
55};
56
65struct Speed { //raw and filtered speed
66 int16_t speed;
67 uint16_t dist;
68};
69
78struct GPSData { //raw lat (deg), long (deg), speed (knots), fix
79 boolean fix = false;
80 float lat;
81 float lon;
82 float speed;
83};
84
93struct RiderTorque { //raw steer torque and offset
94 uint16_t riderTorque;
96};
97
106struct Steer { //raw steer torque and offset
107 uint16_t steerPos;
108 uint16_t steerVel;
110};
111
120struct ForkDisp { //raw front fork displacement
121 uint16_t forkDisp;
123};
124
133struct ActCurr { //raw actual motor current and offset
134 uint16_t actCurr;
135 float actCurr_offset; //Actual current offset (in A). \details The value is found from a zero procedure at the startup. \see do_zeros()
136};
137
146struct Voltage {
147 uint16_t batVolt;
148 float batVolt_offset = 0;
149};
150
156struct Counter {
157 uint16_t speedSensor = 0;
158 uint16_t imu = 0;
159 uint16_t mag = 0;
160 uint16_t debug = 0;
161 uint16_t log = 0;
162 uint16_t LED = 0;
163};
164
169struct Timing {
170 uint32_t dt_cycle;
171 uint32_t tet;
172 uint32_t dt_debug;
173 uint32_t dt_logger;
174 uint32_t max_tet = 0;
175 uint32_t min_tet = -1;
176};
177
182struct RemoteCtrl { //controller channels
198 int16_t ch[NUM_CH_SBUS] = { ZERO_SBUS };
199 //bool lost_frame = true; // Lost frame. \details The lost frame is true when a frame is lost from the receiver. It is initialize to true (frame is lost at the begginging).
200 uint32_t missing_frame = 0;
201};
202
207struct IOstatus {
209 bool mag, magen;
210 bool dac;
212 bool gps;
213 bool sbus;
214};
215
216//global variables
217uint32_t sampling_timer = 0;
219uint32_t nanVal = NAN_VAL;
221boolean LEDstate = false;
234
235//global objects
236ISM330DHCXSensor imu(&USED_SPI, CS_IMU, SPISPEED_IMU);
237LIS3MDLSensor mag(&USED_SPI, CS_MAG, SPISPEED_MAG);
238ADC *adc = new ADC();
239Adafruit_GPS gps(&UART_GPS);
240MCP492X dac(&USED_SPI, SPISPEED_DAC, CS_DAC);
241ControlClass ctrl;
242SerialTransfer speedSensor;
243SbusRx sbus(&UART_SBUS);
245
246//other user vars
247int8_t enable_old = 0;
248int8_t enable_new = 0;
249uint32_t test_timer = 0, test_start = 0;
250
251
253
254#endif
#define UART_SBUS
Used UART for SBUS communication. /details The UART used for the SBUS receiver. This runs with a baud...
Definition config.h:48
#define USED_SPI
Used SPI for sensor IO bus.
Definition config.h:36
#define SPISPEED_IMU
Speed of IMU SPI, max is 10e6.
Definition config.h:37
#define UART_GPS
Used UART for GPS sensor bus.
Definition config.h:44
#define SPISPEED_DAC
Speed of MAG SPI, max is 20e6.
Definition config.h:39
#define GPS_EXTBUFSIZE
Extra buffer size for GPS sensor UART.
Definition config.h:47
#define SPISPEED_MAG
Speed of MAG SPI, max is 10e6.
Definition config.h:38
#define NUM_CH_SBUS
Number channels from SBUS.
Definition config.h:159
#define ZERO_SBUS
Zero-value for SBUS.
Definition config.h:162
#define CS_DAC
Chip select pin of DAC.
Definition config.h:81
#define CS_IMU
Chip select pin of IMU.
Definition config.h:79
#define BR_DIR_PIN
Digital out for brake motor direction.
Definition config.h:98
#define BR_STEP_PIN
Digital out for brake motor step.
Definition config.h:99
#define CS_MAG
Chip select pin of MAG.
Definition config.h:80
#define NAN_VAL
NaN value used for invalid data.
Definition config.h:343
Timing timing
Timing of functions.
Definition objects.h:222
uint32_t sampling_timer
Timer for the main cycle.
Definition objects.h:217
Steer steer_raw
Raw data from the steering sensors.
Definition objects.h:229
SerialTransfer speedSensor
SerialTransfer object for speed sensor.
Definition objects.h:242
uint32_t test_timer
Definition objects.h:249
RemoteCtrl remote_raw
Channels from the remote controller.
Definition objects.h:232
ImuData imuData_raw
Raw data from IMU and magnetometer.
Definition objects.h:224
GPSData gpsData_raw
Raw data from the GPS.
Definition objects.h:226
TS4::Stepper brakeMotor(BR_STEP_PIN, BR_DIR_PIN)
Stepper object for the brake motor.
ControlClass ctrl
Control object.
Definition objects.h:241
int8_t LEDmode
LED mode variable.
Definition objects.h:220
uint8_t GPSserial_extra_buffer[GPS_EXTBUFSIZE]
Definition objects.h:218
IOstatus iostatus
IO status flags.
Definition objects.h:233
ActCurr actCurr_raw
Raw data from the motor current sensor.
Definition objects.h:228
Speed speed_raw
Raw data from speed sensor.
Definition objects.h:225
ForkDisp forkDisp_raw
Raw data from the fork-displacement sensor.
Definition objects.h:230
RiderTorque riderTorque_raw
Raw data from the torsiometer.
Definition objects.h:227
Voltage voltage_raw
Raw data from the battery voltage sensor.
Definition objects.h:231
Counter counters
Counters for lower sampling rate.
Definition objects.h:223
int8_t enable_new
Definition objects.h:248
boolean LEDstate
LED state.
Definition objects.h:221
uint32_t nanVal
NaN variable.
Definition objects.h:219
ADC * adc
Pointer to ADC object.
Definition objects.h:238
uint32_t test_start
Definition objects.h:249
int8_t enable_old
Definition objects.h:247
Led mode constants.
Definition objects.h:30
constexpr int8_t DEF
Default mode without signal-logging.
Definition objects.h:31
constexpr int8_t MTPWAIT
MTP wait for USB connected.
Definition objects.h:35
constexpr int8_t MTP
MTP mode.
Definition objects.h:33
constexpr int8_t ERR
Error mode.
Definition objects.h:34
constexpr int8_t LOG
Default mode with signal-logging.
Definition objects.h:32
Actual motor current raw data.
Definition objects.h:133
uint16_t actCurr
Actual current (in bits).
Definition objects.h:134
float actCurr_offset
Definition objects.h:135
Counters for lower sampling rate.
Definition objects.h:156
uint16_t debug
Counter for debugging.
Definition objects.h:160
uint16_t speedSensor
Counter for speed sensor.
Definition objects.h:157
uint16_t imu
Counter for IMU.
Definition objects.h:158
uint16_t LED
Counter for LED blinking.
Definition objects.h:162
uint16_t mag
Counter for magnetometer.
Definition objects.h:159
uint16_t log
Counter for data logging.
Definition objects.h:161
Fork displacement raw data.
Definition objects.h:120
float forkDisp_offset
Fork displacement offset (in mm).
Definition objects.h:122
uint16_t forkDisp
Fork displacement (in bits).
Definition objects.h:121
GPS raw data.
Definition objects.h:78
boolean fix
Fix value.
Definition objects.h:79
float speed
Ground speed (in knots).
Definition objects.h:82
float lon
Longitude (in degrees).
Definition objects.h:81
float lat
Latitude (in degrees).
Definition objects.h:80
Save the IO startu status.
Definition objects.h:207
bool imu
Definition objects.h:208
bool gps
Definition objects.h:212
bool magen
Definition objects.h:209
bool sbus
Definition objects.h:213
bool gyroen
Definition objects.h:208
bool mag
Definition objects.h:209
bool accen
Definition objects.h:208
bool dac
Definition objects.h:210
bool speedsens
Definition objects.h:211
IMU raw data.
Definition objects.h:50
float gyros_offset[3]
Gyrometer offsets (in mdeg/s).
Definition objects.h:54
int32_t mags_mG[3]
Raw magnetic field (in mG).
Definition objects.h:53
int32_t accs_mg[3]
Raw accelerations (in g).
Definition objects.h:51
int32_t gyros_mdps[3]
Raw angular velocities (in mdeg/s).
Definition objects.h:52
Signals from remote controller.
Definition objects.h:182
int16_t ch[NUM_CH_SBUS]
Channels from remote controller.
Definition objects.h:198
uint32_t missing_frame
Number of missing frame.
Definition objects.h:200
Torsiometer raw data.
Definition objects.h:93
float riderTorque_offset
Rider torque offset (in Nm).
Definition objects.h:95
uint16_t riderTorque
Rider torque (in bits).
Definition objects.h:94
Speed raw data.
Definition objects.h:65
int16_t speed
Vehicle speed (in mm/s).
Definition objects.h:66
uint16_t dist
Elapsed distance (in units of 0.25m).
Definition objects.h:67
Steering raw data.
Definition objects.h:106
uint16_t steerPos
Steering angle (in bits).
Definition objects.h:107
uint16_t steerVel
Steering rate (in bits).
Definition objects.h:108
float steerVel_offset
Steering rate offset (in rad/s).
Definition objects.h:109
Time taken by functions.
Definition objects.h:169
uint32_t max_tet
Maximum TET.
Definition objects.h:174
uint32_t dt_logger
Logger time.
Definition objects.h:173
uint32_t tet
Task execution time.
Definition objects.h:171
uint32_t dt_debug
Debug time.
Definition objects.h:172
uint32_t min_tet
Minimum TET.
Definition objects.h:175
uint32_t dt_cycle
Cycle time.
Definition objects.h:170
Battery voltage raw data.
Definition objects.h:146
uint16_t batVolt
Battery voltage (in bits).
Definition objects.h:147
float batVolt_offset
Battery voltage offset (inV).
Definition objects.h:148