SBB Micro
Source code for the self-balancing-bike microcontroller unit (Teensy 4.1-based). 🚀️
Loading...
Searching...
No Matches
config.h
Go to the documentation of this file.
1
10
11#ifndef __CONFIG_H__
12#define __CONFIG_H__
13
14
24//....................................BUSES....................................
25
34
35//SPI
36#define USED_SPI SPI1
37#define SPISPEED_IMU 10e6
38#define SPISPEED_MAG 10e6
39#define SPISPEED_DAC 10e6
40
41//UART
42#define UART_SPEEDSENS Serial7
43#define BAUD_SPEEDSENS 4e6
44#define UART_GPS Serial2
45#define BAUD_GPS_DEF 9600
46#define BAUD_GPS 57600
47#define GPS_EXTBUFSIZE 128
48#define UART_SBUS Serial8
49
50//ADC
51#define ADC_RES 12
52#define ADC_AVERAGE 8
53
54//DAC
55#define DAC_RES 12
56#define DAC_MTR_CH 0
57#define DAC_THROTTLE_CH 1
58
59//PWM
60#define PWM_RES 12
61#define PWM_FREQ 5e3
62#define PWM_MIN 0.1F
63#define PWM_MAX 0.9F
64
66
67//.....................................PINS....................................
68
77
78//SPI CS PINS
79#define CS_IMU 0
80#define CS_MAG 2
81#define CS_DAC 3
82
83//ANALOG PINS
84#define STEER_POS A16
85#define STEER_SPEED A0
86#define ACT_CURR A1
87#define RIDER_TRQ A17
88#define VOLTAGE A15
89#define FORK_DISP A14
90
91//PWM
92#define PWM_PIN 18
93
94//DIGITAL OUT
95#define MTR_EN_PIN 16
96#define MTR_DIR_PIN 17
97#define RELAY_EN_PIN 33
98#define BR_DIR_PIN 21
99#define BR_STEP_PIN 22
100#define BR_SLEEP_PIN 23
101
102//DIGITAL IN
103#define ONOFF_STATE_PIN 36
104
106
107//.............................CONSTANTS & FACTORS.............................
108
117
118//TIMES
119#define SAMPLING_TIME 1e3
120#define SPEED_SAMPLING_FAC 1
121#define IMU_SAMPLING_FAC 1
122#define MAG_SAMPLING_FAC 5
123#define SBUS_SAMPLING_FAC 14
124#define DEF_LED 500
125#define LOG_LED 500
126#define MTP_LED 250
127#define ERR_LED 10000
128#define MTPWAIT_LED 100
129
130//OUTPUT DATA RATE
131#define ACC_ODR 3333
132#define GYRO_ODR 3333
133#define MAG_ODR 200
134
135//GAINS & CONVERSION
136#define SPEED_SCALE 1.0e-3F
137#define DIST_SCALE 0.25F
138#define ACC_SCALE (-1.0e-3F*9.806F)
139#define GYRO_SCALE (1.0e-3F*PI/180.0F)
140#define MAG_SCALE 0.1F
141#define GPSSPEED_SCALE 0.5144F
142#define GAIN_RIDERTRQ 0.141F
143#define OFFSET_RIDERTRQ -(324.11*GAIN_RIDERTRQ)
144#define GAIN_STEERPOS 50.9F/180.0F*PI
145#define OFFSET_STEERPOS -30.7F/180.0F*PI
146#define GAIN_STEERVEL (2.0F*1000.0F/60.0F*2.0F*PI)
147#define OFFSET_STEERVEL (-1000.0F/60.0F*2.0F*PI)
148#define GAIN_ACTCURR 2*MAX_REFCUR
149#define OFFSET_ACTCURR -MAX_REFCUR
150#define GAIN_VOLTAGE 3.3F*1.027F*160.0F/10.0F
151#define OFFSET_VOLTAGE 0.0F
152#define GAIN_FORKDISP -152.935F
153#define OFFSET_FORKDISP 0.107F
154#define GYROX_OFFSET +0.0059F/GYRO_SCALE
155#define GYROY_OFFSET -0.0066F/GYRO_SCALE
156#define GYROZ_OFFSET -0.0080F/GYRO_SCALE
157
158//SBUS
159#define NUM_CH_SBUS 7
160#define MAX_SBUS 1811
161#define MIN_SBUS 172
162#define ZERO_SBUS 992
163#define TRESHOLD_LOGIC_SBUS 1400
164#define TRESHOLD_POS_SBUS 1400
165#define TRESHOLD_NEG_SBUS 600
166#define MAX_MISSING_SBUS 10*SBUS_SAMPLING_FAC
167#define MAX_REF_INPUT 1
168#define MIN_REF_INPUT -1
169#define SBUS_THROTTLE_CH 2
170#define SBUS_ROLL_CH 1
171#define SBUS_EN_CH 5
172#define SBUS_BR_CH 6
173#define SBUS_THR_OR 3
174#define SBUS_SEL_CH 7
175
176//LIMITS
177#define MAX_REFCUR 10
178#define MAX_REFTHROTTLE 1.0F
179#define MIN_VOLTAGE 15
180#define MAX_VOLTAGE 40
181#define MAX_STEER_ANGLE 0.35
182
183//BRAKE MOTOR
184#define MAX_BR_SPEED 10
185#define MAX_BR_ACC 25
186#define BR_STEPS 200
187#define BR_SPEED_RATIO 1
188#define MAX_BR_DISP 12.0F
189
191
192//....................................MACROS....................................
193
200
201//CONVERSIONS
209#define CONVERT_RIDERTRQ_TO_NM(X) (GAIN_RIDERTRQ * X / (powf(2, ADC_RES) - 1) * 3.3F / 5.0F * 1.0e3 + OFFSET_RIDERTRQ) //macro for conversion of rider torque to Nm
210
218#define CONVERT_STEERPOS_TO_RAD(X) (GAIN_STEERPOS * X / (powf(2, ADC_RES) - 1) + OFFSET_STEERPOS) //macro for conversion of steer position to rad
219
227#define CONVERT_STEERVEL_TO_RADPS(X) ((GAIN_STEERVEL * X / (powf(2, ADC_RES) - 1) + OFFSET_STEERVEL)/STEERVEL_RATIO) //macro for conversion of steer velocity to rad/s
228
236#define CONVERT_ACTCURR_TO_A(X) (-(GAIN_ACTCURR * X / (powf(2, ADC_RES) - 1) + OFFSET_ACTCURR)) //macro for conversion of actual motor current to A
237
245#define CONVERT_CURRENT_TO_PWM(X) (PWM_MIN * (powf(2, PWM_RES) - 1) + abs((PWM_MAX - PWM_MIN) * X / MAX_REFCUR * (powf(2, PWM_RES) - 1))) // convert the motor current to PWM value
246
254#define CONVERT_CURRENT_TO_DAC(X) abs(X / MAX_REFCUR * (powf(2, DAC_RES) - 1)) //convert the motor current to DAC value
255
263#define CONVERT_TRHOTTLE_TO_DAC(X) (X / MAX_REFTHROTTLE * (powf(2, DAC_RES) - 1)) //convert the throttle to DAC value
264
272#define CONVERT_VOLTAGE_TO_V(X) (GAIN_VOLTAGE * X / (powf(2, ADC_RES) - 1) + OFFSET_VOLTAGE) //macro for conversion of battery voltage to V
273
281#define CONVERT_FORKDISP_TO_MM(X) (GAIN_FORKDISP * X / (powf(2, ADC_RES) - 1) + OFFSET_FORKDISP) //macro for conversion of front fork displacement to mm
282
291#define CONVERT_CHANNEL_TO_FLOAT(X, XMIN, XMAX) (float(X)-MIN_SBUS)/(MAX_SBUS-MIN_SBUS)*(XMAX-XMIN) + XMIN //macro for conversion of SBUS channels to floats
292
299#define CONVERT_BRLEV_TO_STEPS(X) ((X*BR_STEPS)/BR_SPEED_RATIO)
300
307#define CONVERT_TRHOTTLE_TO_STEPS(X) CONVERT_BRLEV_TO_STEPS(X / MAX_REFTHROTTLE * MAX_BR_DISP)
308
309#define USB_CONNECTED (!bitRead(USB1_PORTSC1,7))
310
311//ENABLES
312#define MTR_EN_STATE HIGH
313#define RELAY_EN_STATE HIGH
314#define BR_SLEEP_STATE HIGH
315#define MTR_ENABLE digitalWriteFast(MTR_EN_PIN, MTR_EN_STATE), delay(100)
316#define RELAY_ENABLE digitalWriteFast(RELAY_EN_PIN, RELAY_EN_STATE), delay(500)
317#define BR_ENABLE digitalWriteFast(BR_SLEEP_PIN, BR_SLEEP_STATE)
318#define BR_DISABLE digitalWriteFast(BR_SLEEP_PIN, !BR_SLEEP_STATE)
319#define LEDON digitalWriteFast(LED_BUILTIN, HIGH)
320#define LEDOFF digitalWriteFast(LED_BUILTIN, LOW)
321
322//POWER MANAGEMENT
323#define POWER_OFF SNVS_LPCR |= (1<<6)
324
326
327//...................................USER PARAMS.................................
328
334
335#define CPU_FREQ 600e6
336#define MTR_CTRL_MODE 1
337#define EN_LONG_CTRL 0
338#define EN_REMOTE_CTRL 0
339#define CW LOW
340#define NUM_ZERO_SAMPLES 500
341#define STEERVEL_RATIO (-36.0*1.25)
342#define FULLEXT_FORKDISP 0
343#define NAN_VAL 0xFFC00000
344#define YINCL_MCUBOX 1.3426
345const float imu_rotMat[3][3] = { {0, -cosf(YINCL_MCUBOX), -sinf(YINCL_MCUBOX)},
346 {-1, 0, 0},
347 {0, sinf(YINCL_MCUBOX), -cosf(YINCL_MCUBOX)} };
348const float mag_rotMat[3][3] = { {cosf(YINCL_MCUBOX), 0, sinf(YINCL_MCUBOX)},
349 {0, -1, 0},
350 {-sinf(YINCL_MCUBOX), 0, cosf(YINCL_MCUBOX)} };
351//#define SWEEP_TEST
352//#define SIN_TEST
353//#define TEST_DURATION 10e3
354//#define SIN_WAVE 4
355//#define IMP_TEST //!< Flag for impulse test. \details Impulse test consists of a cosine-shaped impulse with amplitude #IMP_AMP and duration #IMP_DUR.
356#define IMP_DUR 100
357#define IMP_AMP -10.0
358
359
360
362
363//.......................................EOF.....................................
364
366
367#endif
const float imu_rotMat[3][3]
Rotation matrix from IMU frame to SAE body-fixed frame.
Definition config.h:345
#define YINCL_MCUBOX
Pitch iinclination of MCU box (in rad).
Definition config.h:344
const float mag_rotMat[3][3]
Rotation matrix from MAG frame to SAE body-fixed frame.
Definition config.h:348