/
main.c
160 lines (131 loc) · 4.71 KB
/
main.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
// Author: Joshua Southerland (2015)
#define USE_CROSS_STUDIO_DEBUG
// STM32F405RG (see stm32f4xx.h to change target device)
// you have to uncomment one of the options, I chose:
//#define STM32F427_437xx
// assert_param undefined reference errors are solved by defining USE_STDPERIPH_DRIVER
//#define USE_STDPERIPH_DRIVER
// for startup file, make sure STARTUP_FROM_RESET is defined
// also have to specify the external crystal speed
//#define HSE_VALUE=16000000
#include "stm32f4xx.h"
#include "wallaby.h"
int main()
{
int32_t bemf_vals[4] = {0,0,0,0};
int32_t bemf_vals_filt[4] = {0,0,0,0};
init();
// set up DMA/SPI buffers
init_rx_buffer();
init_tx_buffer();
// set up pid structs
pid_struct pid_structs[4];
{ uint8_t i;
for (i = 0; i < 4; ++i) init_pid_struct(&(pid_structs[i]), i);
}
update_dig_pin_configs();
config_adc_in_from_regs();
//debug_printf("starting\n");
int low_volt_alarmed = 0;
// Loop until button is pressed
uint32_t count = 0;
while (1)
{
count += 1;
{
// only sample motor backemf 1/4 of the time
const uint8_t bemf_update_time = (count % 4 == 1);
if (bemf_update_time)
{
// idle breaking
MOT0_DIR1_PORT->BSRRH |= MOT0_DIR1;
MOT0_DIR2_PORT->BSRRH |= MOT0_DIR2;
MOT1_DIR1_PORT->BSRRH |= MOT1_DIR1;
MOT1_DIR2_PORT->BSRRH |= MOT1_DIR2;
MOT2_DIR1_PORT->BSRRH |= MOT2_DIR1;
MOT2_DIR2_PORT->BSRRH |= MOT2_DIR2;
MOT3_DIR1_PORT->BSRRH |= MOT3_DIR1;
MOT3_DIR2_PORT->BSRRH |= MOT3_DIR2;
}
// let the motor coast
// delay_us(700);
// now I squeeze in most sensor updates instead of just sleeping
uint32_t before = usCount;
update_dig_pins();
int16_t batt = adc_update();
readAccel();
readMag();
readGyro();
if (batt < 636) // about 5.75 volts
{
configDigitalOutPin(LED1_PIN, LED1_PORT);
low_volt_alarmed = 1;
if (count % 50 < 10) // low duty cycle to save battery
{
LED1_PORT->BSRRL |= LED1_PIN; // ON
}
else
{
LED1_PORT->BSRRH |= LED1_PIN; // OFF
}
}
else if (low_volt_alarmed)
{
// make sure led is off coming out of the low voltage alarm
configDigitalOutPin(LED1_PIN, LED1_PORT);
LED1_PORT->BSRRH |= LED1_PIN; // OFF
low_volt_alarmed = 0;
}
if (adc_dirty)
{
adc_dirty = 0;
config_adc_in_from_regs();
}
if (dig_dirty)
{
dig_dirty = 0;
update_dig_pin_configs();
}
uint32_t sensor_update_time = usCount - before;
const uint16_t us_delay_needed = 700;
const uint8_t got_time_to_burn = sensor_update_time < us_delay_needed;
if (got_time_to_burn)
{
// sleep the remainder of the coasting period before sampling bemf
delay_us(us_delay_needed-sensor_update_time);
}
if (bemf_update_time)
{
update_bemfs(bemf_vals, bemf_vals_filt);
// set the motor directions back up
update_motor_modes();
uint8_t channel;
for (channel = 0; channel < 4; ++channel)
{
uint8_t shift = 2*channel;
uint8_t motor_mode = (aTxBuffer[REG_RW_MOT_MODES] & (0b11 << shift)) >> shift;
motor_update(bemf_vals[channel], bemf_vals_filt[channel], &pid_structs[channel], channel, motor_mode);
}
}
else
{
// updating sensors doesnt' take as long as all of the adc for backemf updates
// sleep a bit so that our time through the loop is consistent for PID control
delay_us(222);
}
}
}
// set all motor pwms to 0
aTxBuffer[REG_RW_MOT_0_PWM_H] = 0;
aTxBuffer[REG_RW_MOT_0_PWM_L] = 0;
aTxBuffer[REG_RW_MOT_1_PWM_H] = 0;
aTxBuffer[REG_RW_MOT_1_PWM_L] = 0;
aTxBuffer[REG_RW_MOT_2_PWM_H] = 0;
aTxBuffer[REG_RW_MOT_2_PWM_L] = 0;
aTxBuffer[REG_RW_MOT_3_PWM_H] = 0;
aTxBuffer[REG_RW_MOT_3_PWM_L] = 0;
// set all motors to idle state
idle_motor_dirs();
spi2_dma_cleanup();
//debug_printf("done\n");
}