232 lines
7.3 KiB
C
232 lines
7.3 KiB
C
|
#include <stdio.h>
|
|||
|
#include <string.h>
|
|||
|
#include "globals.h"
|
|||
|
#include "gui.h"
|
|||
|
#include "hardware.h"
|
|||
|
#include "kugoo_s3.h"
|
|||
|
#include "persistence.h"
|
|||
|
#include "ssd1306.h"
|
|||
|
#include "utils.h"
|
|||
|
|
|||
|
|
|||
|
void usart1_isr(void) {
|
|||
|
// while data is not empty
|
|||
|
while (USART_SR(USART1) & USART_SR_RXNE) {
|
|||
|
kugoo_s3_byte_received(usart_recv(USART1));
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
int main(void) {
|
|||
|
reset_reason = RCC_CSR;
|
|||
|
RCC_CSR |= RCC_CSR_RMVF;
|
|||
|
|
|||
|
memset((void *)&kugoo_s3_rx, 0, sizeof(struct kugoo_s3_packet_in));
|
|||
|
uint32_t next_packet_send_time;
|
|||
|
uint32_t next_logic_refresh;
|
|||
|
uint32_t next_throttle_lock_cancellation = 0;
|
|||
|
uint16_t throttle_trigger = 0;
|
|||
|
uint8_t prev_throttle_lock_state = 0;
|
|||
|
uint8_t throttle_locked = 0;
|
|||
|
uint8_t zero_packets_left = 0; // anti throttle lock zero packets
|
|||
|
enum control_state_t control_state = CS_NORMAL;
|
|||
|
|
|||
|
float desired_speed_kmh = 0;
|
|||
|
float control_output_kmh = 0;
|
|||
|
float speed_error = 0;
|
|||
|
float current_error = 0;
|
|||
|
float speed = 0;
|
|||
|
|
|||
|
hardware_setup();
|
|||
|
|
|||
|
eeprom_read_and_validate();
|
|||
|
|
|||
|
beep_blocking(1000, 10, storage.keys_volume);
|
|||
|
|
|||
|
ssd1306_init();
|
|||
|
ssd1306_contrast(0xf0);
|
|||
|
ssd1306_blend_mode(SSD1306_BLEND_MODE_LIGHTEN);
|
|||
|
ssd1306_fill(0xff); // screen test
|
|||
|
ssd1306_redraw();
|
|||
|
msleep(200);
|
|||
|
|
|||
|
|
|||
|
if (beep_button_pressed()) {
|
|||
|
ssd1306_clear();
|
|||
|
ssd1306_string("СБРОС НАСТРОЕК\n\n"
|
|||
|
"СОХРАНИТЬ ОДОМЕТР:\n"
|
|||
|
"SET\n\n"
|
|||
|
"СТЕРЕТЬ ВСЁ:\n"
|
|||
|
"LIGHT");
|
|||
|
ssd1306_redraw();
|
|||
|
|
|||
|
while (1) {
|
|||
|
if (set_button_pressed()) {
|
|||
|
uint32_t distance = storage.total_distance_meters;
|
|||
|
eeprom_reset_all(false);
|
|||
|
storage.total_distance_meters = distance;
|
|||
|
eeprom_write_all();
|
|||
|
beep_blocking(2000, 300, STATIC_BEEP_VOLUME);
|
|||
|
power_disable();
|
|||
|
} else if (light_button_pressed()) {
|
|||
|
eeprom_reset_all(true);
|
|||
|
beep_blocking(2000, 300, STATIC_BEEP_VOLUME);
|
|||
|
power_disable();
|
|||
|
} else if (power_button_pressed()) {
|
|||
|
break;
|
|||
|
}
|
|||
|
msleep(20);
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
while (power_button_pressed()) {
|
|||
|
msleep(20);
|
|||
|
}
|
|||
|
|
|||
|
kugoo_s3_packet_init(&kugoo_s3_tx);
|
|||
|
next_packet_send_time = millis() + PACKET_SEND_INTERVAL_MS;
|
|||
|
next_logic_refresh = millis() + LOGIC_REFRESH_INTERVAL_MS;
|
|||
|
|
|||
|
beep_blocking(2000, 10, storage.keys_volume);
|
|||
|
|
|||
|
while (1) {
|
|||
|
|
|||
|
// update only when new speed and current received
|
|||
|
if (kugoo_s3_tx_ready) {
|
|||
|
kugoo_s3_tx_ready = false;
|
|||
|
|
|||
|
// control loop
|
|||
|
speed = kugoo_s3_get_speed();
|
|||
|
speed_error = desired_speed_kmh - speed;
|
|||
|
current_error = storage.current_limit - ((float) kugoo_s3_rx.current / 10.0f);
|
|||
|
|
|||
|
// when current overflows maximum current, switch to current limiting
|
|||
|
// instead of speed stabilization
|
|||
|
if(control_state == CS_NORMAL) {
|
|||
|
if (storage.current_limit_enabled &&
|
|||
|
storage.current_limit > 0 &&
|
|||
|
current_error < 0) {
|
|||
|
control_output_kmh += current_error * storage.current_stabilization_Kp;
|
|||
|
} else if(storage.speed_stabilization_enabled) {
|
|||
|
control_output_kmh += speed_error * storage.speed_stabilization_Kp;
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
try_persist_distance();
|
|||
|
}
|
|||
|
|
|||
|
if (millis() >= next_logic_refresh) {
|
|||
|
next_logic_refresh = millis() + LOGIC_REFRESH_INTERVAL_MS;
|
|||
|
iwdg_reset();
|
|||
|
process_events();
|
|||
|
|
|||
|
speed = kugoo_s3_get_speed();
|
|||
|
|
|||
|
if(gui_is_view(GUI_VIEW_TRIGGER_CALIBRATION)){
|
|||
|
throttle_trigger = 0;
|
|||
|
kugoo_s3_tx.brake = 0;
|
|||
|
} else {
|
|||
|
throttle_trigger = throttle_value_normalized();
|
|||
|
kugoo_s3_tx.brake = brake_value_normalized();
|
|||
|
}
|
|||
|
|
|||
|
throttle_locked = kugoo_s3_rx.state & KUGOO_S3_STATE_THROTTLE_LOCKED;
|
|||
|
if (prev_throttle_lock_state != throttle_locked) {
|
|||
|
prev_throttle_lock_state = throttle_locked;
|
|||
|
if (throttle_locked && !storage.anti_throttle_lock) {
|
|||
|
beep(2000, 10, storage.signals_volume);
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
// factory cruise control cancellation
|
|||
|
// sometimes doesn't work
|
|||
|
// fuck this
|
|||
|
if (throttle_locked && storage.anti_throttle_lock && millis() > next_throttle_lock_cancellation) {
|
|||
|
if(control_state == CS_NORMAL) {
|
|||
|
next_throttle_lock_cancellation = millis() + ANTI_THROTTLE_LOCK_PERIOD_MS;
|
|||
|
control_state = CS_FORCE_ZERO_THROTTLE;
|
|||
|
zero_packets_left = ANTI_THROTTLE_LOCK_ZERO_PACKETS;
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
// todo: reduce complexity
|
|||
|
if (control_state == CS_THROTTLE_RECOVER) {
|
|||
|
desired_speed_kmh += THROTTLE_RECOVER_INCREMENT;
|
|||
|
if (cruise_ctl_status == CRUISE_CTL_DISABLED) {
|
|||
|
if (desired_speed_kmh >= throttle_value_to_kmh()) {
|
|||
|
desired_speed_kmh = throttle_value_to_kmh();
|
|||
|
control_state = CS_NORMAL;
|
|||
|
}
|
|||
|
} else {
|
|||
|
if (desired_speed_kmh >= cruise_ctl_speed_kmh) {
|
|||
|
desired_speed_kmh = cruise_ctl_speed_kmh;
|
|||
|
control_state = CS_NORMAL;
|
|||
|
}
|
|||
|
}
|
|||
|
} else if (cruise_ctl_status == CRUISE_CTL_DISABLED) {
|
|||
|
if (throttle_trigger > 10) { // throttle trigger pressed
|
|||
|
if (control_state == CS_FORCE_ZERO_THROTTLE) {
|
|||
|
// do nothing
|
|||
|
} else if (storage.soft_start_enabled) {
|
|||
|
if (desired_speed_kmh < speed / 2) {
|
|||
|
desired_speed_kmh =
|
|||
|
speed / 2; // do not start with zero when speed > 0
|
|||
|
} else if (desired_speed_kmh + storage.soft_start_increment <=
|
|||
|
throttle_value_to_kmh()) {
|
|||
|
desired_speed_kmh += storage.soft_start_increment;
|
|||
|
} else {
|
|||
|
desired_speed_kmh = throttle_value_to_kmh();
|
|||
|
}
|
|||
|
} else {
|
|||
|
desired_speed_kmh = throttle_value_to_kmh();
|
|||
|
}
|
|||
|
} else { // throttle trigger not pressed
|
|||
|
desired_speed_kmh = 0;
|
|||
|
control_output_kmh = 0;
|
|||
|
}
|
|||
|
} else if (cruise_ctl_status == CRUISE_CTL_WAITING_RELEASE && throttle_trigger < 50) {
|
|||
|
desired_speed_kmh = cruise_ctl_speed_kmh;
|
|||
|
cruise_ctl_status = CRUISE_CTL_ENABLED;
|
|||
|
} else if (cruise_ctl_status == CRUISE_CTL_ENABLED && throttle_trigger > 50) {
|
|||
|
desired_speed_kmh = cruise_ctl_speed_kmh;
|
|||
|
cruise_ctl_status = CRUISE_CTL_DISABLED;
|
|||
|
beep(2000, 10, storage.signals_volume);
|
|||
|
}
|
|||
|
|
|||
|
if (kugoo_s3_tx.brake > 50) {
|
|||
|
cruise_ctl_status = CRUISE_CTL_DISABLED;
|
|||
|
control_output_kmh = 0;
|
|||
|
}
|
|||
|
|
|||
|
// Do not run throttle lock cancellation while idle
|
|||
|
if(desired_speed_kmh == 0) {
|
|||
|
next_throttle_lock_cancellation = millis() + ANTI_THROTTLE_LOCK_PERIOD_MS;
|
|||
|
}
|
|||
|
|
|||
|
if(control_state == CS_FORCE_ZERO_THROTTLE && zero_packets_left > 0){
|
|||
|
kugoo_s3_tx.throttle = 0;
|
|||
|
if(--zero_packets_left == 0) {
|
|||
|
control_state = CS_NORMAL;
|
|||
|
}
|
|||
|
} else {
|
|||
|
kugoo_s3_set_speed_approx(desired_speed_kmh + control_output_kmh);
|
|||
|
}
|
|||
|
|
|||
|
kugoo_s3_tx.features = KUGOO_S3_FEATURE_UNLOCK;
|
|||
|
if (storage.zero_start_enabled) {
|
|||
|
kugoo_s3_tx.features |= KUGOO_S3_FEATURE_ZERO_START;
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
if (millis() >= next_packet_send_time) {
|
|||
|
next_packet_send_time = millis() + PACKET_SEND_INTERVAL_MS;
|
|||
|
kugoo_s3_packet_send(&kugoo_s3_tx);
|
|||
|
// printf("%d;%d;%d;%d;%d\r\n", throttle, (int)desired_speed_kmh, kugoo_s3_tx.throttle, (int)kugoo_s3_get_speed(), (int)(kugoo_s3_rx.current/10));
|
|||
|
}
|
|||
|
|
|||
|
gui_redraw();
|
|||
|
}
|
|||
|
|
|||
|
return 0;
|
|||
|
}
|