#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <cstdio>
#include <cstring>
#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "hardware/pwm.h"
#include "pico/multicore.h"
#include "hardware/uart.h"
#include "hardware/irq.h"
#include "hardware/adc.h"
#define GPIO_PICO_LED 25
#define UART_TX_PIN 0
#define UART_RX_PIN 1
#define GPIO_ENC_A 2
#define GPIO_ENC_B 3
#define GPIO_DRIVE_PWM 4
#define GPIO_DRIVE_CW 5
#define GPIO_DRIVE_CCW 6
#define GPIO_SERVO1_PWM 7
#define GPIO_SERVO1_ENABLE 10
#define GPIO_SERVO2_PWM 11
#define GPIO_SERVO2_ENABLE 12
#define GPIO_DIST1_TRG 20
#define GPIO_DIST1_ECH 21
#define GPIO_ADC_VSYS 29
#define GPIO_ADC_BATT 26
#define GPIO_ADC_SERVO2 27
#define IRQ_enca_enable gpio_set_irq_enabled(GPIO_ENC_A, GPIO_IRQ_EDGE_RISE, true);
#define IRQ_enca_disable gpio_set_irq_enabled(GPIO_ENC_A, GPIO_IRQ_EDGE_RISE, false);
#define UART_ID uart0
#define BAUD_RATE 19200
#define DATA_BITS 8
#define STOP_BITS 1
#define PARITY UART_PARITY_NONE
#define UART_SEND_ERR_MESSAGE uart_puts(UART_ID,"uart,error\n");
#define ERROR 1
#define NON_ERROR 0
#define SEND_TOTAL_FAULT "total_fault"
int g_set_rps = 0, g_enable_break=0,g_auto_break_flg=0;
long long int g_enca_val = 0, g_encb_val = 0, g_enc_count_total = 0, g_enc_count_sub = 0,g_stop_rot = 0,g_count_rot = 0;
float g_kp=3, g_ki=0.1, g_kd=0.005, g_drive_rps = 0, g_pid_duty = 0 , g_drive_rps_ave = 0;;
int g_drive_pwm_clkdiv = 25, g_drive_pwm_warp = 49999;
const int g_servo1_duty100 = 49999;
const int g_servo2_duty100 = 49999;
int g_trq_error_flg = 0, g_trq_error_chk = 1;
int chars_rxed = 0;
int uart_flg = 0;
char uart_rx_data[64];
const float adc_conv_fact = 3.3f / (1 << 12);
uint pico_led_freq = 100;
float g_Vbat,g_Vbat_max,g_Vbat_min,g_Vsys,g_Vsys_max,g_Vsys_min,g_temp,g_temp_max,g_temp_min;
int g_total_fault = 0;
void enc_callback(uint gpio, uint32_t events) {
g_encb_val = gpio_get(GPIO_ENC_B);
g_enca_val = gpio_get(GPIO_ENC_A);
if (gpio == GPIO_ENC_A && events == GPIO_IRQ_EDGE_RISE){
if(g_encb_val == 0){
g_enc_count_total++;
g_enc_count_sub++;
}else{
g_enc_count_total--;
g_enc_count_sub--;
}
}else if (gpio == GPIO_ENC_A && events == GPIO_IRQ_EDGE_FALL){
if(g_encb_val == 1){
g_enc_count_total++;
g_enc_count_sub++;
}else{
g_enc_count_total--;
g_enc_count_sub--;
}
}else if (gpio == GPIO_ENC_B && events == GPIO_IRQ_EDGE_RISE){
if(g_enca_val == 1){
g_enc_count_total++;
g_enc_count_sub++;
}else{
g_enc_count_total--;
g_enc_count_sub--;
}
}else if (gpio == GPIO_ENC_B && events == GPIO_IRQ_EDGE_FALL){
if(g_enca_val == 0){
g_enc_count_total++;
g_enc_count_sub++;
}else{
g_enc_count_total--;
g_enc_count_sub--;
}
}
}
bool drive_motor(){
static uint32_t stop_st = 0;
uint32_t stop_t = 0;
if (g_total_fault == 1){
gpio_put(GPIO_DRIVE_CW, 0);
gpio_put(GPIO_DRIVE_CCW, 0);
pwm_set_gpio_level(GPIO_DRIVE_PWM, 0);
g_set_rps = 0;
return true;
}
if (g_trq_error_flg == 1){
gpio_put(GPIO_DRIVE_CW, 0);
gpio_put(GPIO_DRIVE_CCW, 0);
pwm_set_gpio_level(GPIO_DRIVE_PWM, 0);
g_set_rps = 0;
return true;
}
if(g_enable_break == 1 ){
pwm_set_gpio_level(GPIO_DRIVE_PWM, g_drive_pwm_warp);
gpio_put(GPIO_DRIVE_CW, 1);
gpio_put(GPIO_DRIVE_CCW, 1);
g_set_rps = 0;
return true;
}
if(g_auto_break_flg == 1 ){
pwm_set_gpio_level(GPIO_DRIVE_PWM, g_drive_pwm_warp);
gpio_put(GPIO_DRIVE_CW, 1);
gpio_put(GPIO_DRIVE_CCW, 1);
g_set_rps = 0;
return true;
}
else if (g_set_rps==0){
pwm_set_gpio_level(GPIO_DRIVE_PWM, 0);
gpio_put(GPIO_DRIVE_CW, 0);
gpio_put(GPIO_DRIVE_CCW, 0);
return true;
}
else if (((g_stop_rot > 0) && (g_count_rot > g_stop_rot))||((g_stop_rot < 0) && (g_count_rot < g_stop_rot))){
g_auto_break_flg = 1;
gpio_put(GPIO_DRIVE_CW, 1);
gpio_put(GPIO_DRIVE_CCW, 1);
pwm_set_gpio_level(GPIO_DRIVE_PWM, g_drive_pwm_warp);
g_set_rps = 0;
return true;
}
if (g_pid_duty > 0){
gpio_put(GPIO_DRIVE_CW, 1);
gpio_put(GPIO_DRIVE_CCW, 0);
}
else if (g_pid_duty < 0){
gpio_put(GPIO_DRIVE_CW, 0);
gpio_put(GPIO_DRIVE_CCW, 1);
}
pwm_set_gpio_level(GPIO_DRIVE_PWM, g_drive_pwm_warp*(abs(g_pid_duty)/100.f));
if (abs(g_pid_duty) >= 99.f && abs(g_drive_rps) < 0.5f && g_trq_error_chk == 1){
if (stop_st == 0){
stop_st = to_ms_since_boot(get_absolute_time());
return true;
}else{
stop_t = to_ms_since_boot(get_absolute_time()) - stop_st;
if (stop_t > 2000){
g_trq_error_flg =1 ;
g_set_rps = 0;
return true;
}
return true;
}
}
stop_st = 0;
return true;
}
bool repeating_timer_callback(struct repeating_timer *t) {
const int rot_pls = 52;
static uint32_t pretime = 0;
static float rps_arr[50]={};
static int ave_count=0;
float rps_sum = 0;
g_drive_rps = ((float)g_enc_count_sub / (float)rot_pls)*100;
g_count_rot = (float)g_enc_count_total / (float)rot_pls;
g_enc_count_sub = 0;
if (ave_count == 50){
ave_count = 0;
}
rps_arr[ave_count] = g_drive_rps;
ave_count++;
for (int i = 0; i < 50; i++){
rps_sum += rps_arr[i];
}
g_drive_rps_ave = rps_sum / 50;
static float P,I,D,dt,preP=0,duty,bef_set_rps=0;
dt = to_ms_since_boot(get_absolute_time()) - pretime;
pretime = to_ms_since_boot(get_absolute_time());
preP = P;
P = g_drive_rps - (float)g_set_rps;
I += (P + preP) / 2.0 * dt;
if (g_set_rps == 0){
I = 0;
}
if (g_set_rps != bef_set_rps){
I = 0;
}
bef_set_rps = g_set_rps;
D = (P - preP) / dt;
duty = (g_kp * P + g_kd * D + g_ki * I)* 1;
if (duty>=100){
duty = 100.0;
}
else if (duty<=-100.0){
duty = -100.0;
}
g_pid_duty = duty;
drive_motor();
return true;
}
void setup_enc(void){
gpio_init(GPIO_ENC_A);
gpio_init(GPIO_ENC_B);
gpio_set_dir(GPIO_ENC_A, GPIO_IN);
gpio_set_dir(GPIO_ENC_B, GPIO_IN);
gpio_pull_down(GPIO_ENC_A);
gpio_pull_down(GPIO_ENC_B);
}
void setup_drive_motor(void){
gpio_set_function(GPIO_DRIVE_PWM, GPIO_FUNC_PWM);
uint slice_num_drive = pwm_gpio_to_slice_num(GPIO_DRIVE_PWM);
pwm_set_clkdiv(slice_num_drive, g_drive_pwm_clkdiv);
pwm_set_wrap(slice_num_drive, g_drive_pwm_warp);
pwm_set_gpio_level(GPIO_DRIVE_PWM, 0);
pwm_set_enabled(slice_num_drive, true);
gpio_init(GPIO_DRIVE_CW);
gpio_init(GPIO_DRIVE_CCW);
gpio_set_dir(GPIO_DRIVE_PWM, GPIO_OUT);
gpio_set_dir(GPIO_DRIVE_CW, GPIO_OUT);
gpio_set_dir(GPIO_DRIVE_CCW, GPIO_OUT);
gpio_put(GPIO_DRIVE_CW, 0);
gpio_put(GPIO_DRIVE_CCW, 0);
}
void setup_servo_motor(void){
gpio_set_function(GPIO_SERVO1_PWM, GPIO_FUNC_PWM);
gpio_set_function(GPIO_SERVO2_PWM, GPIO_FUNC_PWM);
uint slice_num_servo1 = pwm_gpio_to_slice_num(GPIO_SERVO1_PWM);
uint slice_num_servo2 = pwm_gpio_to_slice_num(GPIO_SERVO2_PWM);
pwm_set_clkdiv(slice_num_servo1, 50);
pwm_set_clkdiv(slice_num_servo2, 50);
pwm_set_wrap(slice_num_servo1, g_servo1_duty100);
pwm_set_wrap(slice_num_servo2, g_servo2_duty100);
pwm_set_enabled(slice_num_servo1, true);
pwm_set_enabled(slice_num_servo2, true);
gpio_init(GPIO_SERVO1_ENABLE);
gpio_init(GPIO_SERVO2_ENABLE);
gpio_set_dir(GPIO_SERVO1_PWM, GPIO_OUT);
gpio_set_dir(GPIO_SERVO1_ENABLE, GPIO_OUT);
gpio_set_dir(GPIO_SERVO2_PWM, GPIO_OUT);
gpio_set_dir(GPIO_SERVO2_ENABLE, GPIO_OUT);
gpio_put(GPIO_SERVO1_ENABLE, 0);
gpio_put(GPIO_SERVO2_ENABLE, 0);
pwm_set_gpio_level(GPIO_SERVO1_PWM, 0);
pwm_set_gpio_level(GPIO_SERVO2_PWM, 0);
}
void setup_dist_wave(void){
gpio_init(GPIO_DIST1_ECH);
gpio_init(GPIO_DIST1_TRG);
gpio_set_dir(GPIO_DIST1_TRG, GPIO_OUT);
gpio_set_dir(GPIO_DIST1_ECH, GPIO_IN);
gpio_put(GPIO_DIST1_TRG, 0);
}
void on_uart_rx(void){
uart_set_irq_enables(UART_ID, false, false);
while (uart_is_readable(UART_ID)) {
char ch = uart_getc(UART_ID);
uart_rx_data[chars_rxed] = ch;
if (uart_is_writable(UART_ID)) {
ch++;
}
}
if (uart_rx_data[chars_rxed] == '\n'){
uart_flg = 1;
}else{
chars_rxed++;
}
uart_set_irq_enables(UART_ID, true, false);
}
void setup_uart(void){
uart_init(UART_ID, BAUD_RATE);
gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART);
gpio_set_function(UART_RX_PIN, GPIO_FUNC_UART);
int actual = uart_set_baudrate(UART_ID, BAUD_RATE);
uart_set_hw_flow(UART_ID, false, false);
uart_set_format(UART_ID, DATA_BITS, STOP_BITS, PARITY);
uart_set_fifo_enabled(UART_ID, false);
int UART_IRQ = UART_ID == uart0 ? UART0_IRQ : UART1_IRQ;
irq_set_exclusive_handler(UART_IRQ, on_uart_rx);
irq_set_enabled(UART_IRQ, true);
uart_set_irq_enables(UART_ID, true, false);
}
void setup_adc(void){
adc_init();
adc_set_temp_sensor_enabled(true);
adc_gpio_init(GPIO_ADC_VSYS);
adc_gpio_init(GPIO_ADC_BATT);
adc_gpio_init(GPIO_ADC_SERVO2);
}
float get_temp(){
adc_select_input(4);
uint16_t adc_val = adc_read();
float volt = adc_val * adc_conv_fact;
float temp = 27 - (volt - 0.706) / 0.001721;
return temp;
}
float get_vsys(){
adc_select_input(3);
uint16_t adc_val = adc_read();
float volt = adc_val * adc_conv_fact * 3;
return volt;
}
float get_batt(){
adc_select_input(0);
uint16_t adc_val = adc_read();
float volt = adc_val * adc_conv_fact * 10.8650693;
return volt;
}
int get_srv2_angle(){
adc_select_input(1);
const float angle_r90 = 0.183243;
const float angle_l90 = 3.019942;
const float angle_0 = 1.603495;
const float angle_1 = (angle_l90 - angle_r90)/ 180.f;
uint16_t adc_val = adc_read();
float volt = adc_val * adc_conv_fact;
int angle = (angle_0 - volt) / angle_1;
return angle;
}
int steering(int angle=0){
const float center = 3775;
const float max_val = 700;
if (angle == 999){
gpio_put(GPIO_SERVO1_ENABLE, 0);
pwm_set_gpio_level(GPIO_SERVO1_PWM, 0);
return NON_ERROR;
}
if (angle > 100)
{
angle = 100;
}else if (angle < -100){
angle = -100;
}
int set_pwm = center - max_val * (float(angle) / 100.f);
gpio_put(GPIO_SERVO1_ENABLE, 1);
pwm_set_gpio_level(GPIO_SERVO1_PWM, set_pwm);
return NON_ERROR;
}
int set_distance_angle(int angle=0){
const float center = 3624;
const float max_val = 2100;
uint32_t pretime,dt;
if (angle == 999){
gpio_put(GPIO_SERVO2_ENABLE, 0);
pwm_set_gpio_level(GPIO_SERVO2_PWM, 0);
return NON_ERROR;
}
if (angle > 90)
{
angle = 90;
}else if (angle < -90){
angle = -90;
}
int set_pwm = center - max_val * (float(angle) / 90.f);
gpio_put(GPIO_SERVO2_ENABLE, 1);
pwm_set_gpio_level(GPIO_SERVO2_PWM, set_pwm);
return NON_ERROR;
}
int mes_distance(){
uint64_t trig_on_t,trig_off_t,echo_off_t,echo_on_t;
gpio_put(GPIO_DIST1_TRG, 1);
trig_on_t = to_us_since_boot(get_absolute_time());
trig_off_t = trig_on_t;
while (trig_off_t - trig_on_t< 20){
trig_off_t = to_us_since_boot(get_absolute_time());
}
gpio_put(GPIO_DIST1_TRG, 0);
while (gpio_get(GPIO_DIST1_ECH) == 0){
echo_off_t = to_us_since_boot(get_absolute_time());
if (echo_off_t - trig_off_t > 30000){
return -1;
}
}
while (gpio_get(GPIO_DIST1_ECH) == 1){
echo_on_t = to_us_since_boot(get_absolute_time());
if (echo_on_t - echo_off_t >= 30000){
return -999;
}
}
int dist = (float(echo_on_t - echo_off_t)* (0.3315 + 0.0006 * g_temp)) / 2.f;
return dist;
}
int command_processing(char cmd_ar[],char cmd_data_ar[]){
char send_data[32]={};
if (strcmp(cmd_ar,"get_total_fault") == 0 ){
snprintf(send_data, sizeof(send_data), "%d", g_total_fault);
}
else if (strcmp(cmd_ar,"get_rps") == 0 ){
snprintf(send_data, sizeof(send_data), "%f", g_drive_rps);
}
else if (strcmp(cmd_ar,"get_rps_ave") == 0 ){
snprintf(send_data, sizeof(send_data), "%f", g_drive_rps_ave);
}
else if (strcmp(cmd_ar,"get_count_rot") == 0 ){
snprintf(send_data, sizeof(send_data), "%lld", g_count_rot);
}
else if (strcmp(cmd_ar,"clr_count_rot") == 0 ){
g_enc_count_total = 0;
g_count_rot = 0;
snprintf(send_data, sizeof(send_data), "%lld", g_count_rot);
}
else if (strcmp(cmd_ar,"set_rps") == 0 ){
g_set_rps = atoi(cmd_data_ar);
snprintf(send_data, sizeof(send_data), "%d", g_set_rps);
}
else if (strcmp(cmd_ar,"set_stop_rot") == 0 ){
g_stop_rot = atoi(cmd_data_ar);
snprintf(send_data, sizeof(send_data), "%lld", g_stop_rot);
}
else if (strcmp(cmd_ar,"get_drive_duty") == 0){
snprintf(send_data, sizeof(send_data), "%3.1f", g_pid_duty);
}
else if (strcmp(cmd_ar,"set_pid_kp") == 0){
g_kp = atof(cmd_data_ar);
snprintf(send_data, sizeof(send_data), "%f", g_kp);
}
else if (strcmp(cmd_ar,"set_pid_ki") == 0){
g_ki = atof(cmd_data_ar);
snprintf(send_data, sizeof(send_data), "%f", g_ki);
}
else if (strcmp(cmd_ar,"set_pid_kd") == 0){
g_kd = atof(cmd_data_ar);
snprintf(send_data, sizeof(send_data), "%f", g_kd);
}
else if (strcmp(cmd_ar,"get_pid_kp") == 0){
snprintf(send_data, sizeof(send_data), "%f", g_kp);
}
else if (strcmp(cmd_ar,"get_pid_ki") == 0){
snprintf(send_data, sizeof(send_data), "%f", g_ki);
}
else if (strcmp(cmd_ar,"get_pid_kd") == 0){
snprintf(send_data, sizeof(send_data), "%f", g_kd);
}
else if (strcmp(cmd_ar,"set_trq_err") == 0){
if (atoi(cmd_data_ar) == 0){
g_trq_error_chk = 0;
g_trq_error_flg = 0;
}else{
g_trq_error_chk = 1;
g_trq_error_flg = 0;
}
snprintf(send_data, sizeof(send_data), "%d", g_trq_error_chk);
}
else if (strcmp(cmd_ar,"clr_trq_err") == 0){
g_trq_error_flg = 0;
snprintf(send_data, sizeof(send_data), "%d", g_trq_error_flg);
}
else if (strcmp(cmd_ar,"get_trq_err") == 0){
snprintf(send_data, sizeof(send_data), "%d", g_trq_error_flg);
}
else if (strcmp(cmd_ar,"set_dist_angle") == 0){
int angle = atoi(cmd_data_ar);
set_distance_angle(angle);
snprintf(send_data, sizeof(send_data), "%d", angle);
}
else if (strcmp(cmd_ar,"get_dist_angle") == 0){
int angle = get_srv2_angle();
snprintf(send_data, sizeof(send_data), "%d", angle);
}
else if (strcmp(cmd_ar,"get_dist") == 0){
int dist = mes_distance();
snprintf(send_data, sizeof(send_data), "%d", dist);
}
else if (strcmp(cmd_ar,"set_steering") == 0){
int angle = atoi(cmd_data_ar);
steering(angle);
snprintf(send_data, sizeof(send_data), "%d", angle);
}
else if (strcmp(cmd_ar,"get_auto_break_flg") == 0){
snprintf(send_data, sizeof(send_data), "%d", g_auto_break_flg);
}
else if (strcmp(cmd_ar,"clr_auto_break_flg") == 0){
g_auto_break_flg = 0;
snprintf(send_data, sizeof(send_data), "%d", g_auto_break_flg);
}
else if (strcmp(cmd_ar,"get_temp") == 0){
snprintf(send_data, sizeof(send_data), "%f", g_temp);
}
else if (strcmp(cmd_ar,"get_temp_max") == 0){
snprintf(send_data, sizeof(send_data), "%f", g_temp_max);
}
else if (strcmp(cmd_ar,"get_temp_min") == 0){
snprintf(send_data, sizeof(send_data), "%f", g_temp_min);
}
else if (strcmp(cmd_ar,"get_Vsys") == 0){
snprintf(send_data, sizeof(send_data), "%f", g_Vsys);
}
else if (strcmp(cmd_ar,"get_Vsys_max") == 0){
snprintf(send_data, sizeof(send_data), "%f", g_Vsys_max);
}
else if (strcmp(cmd_ar,"get_Vsys_min") == 0){
snprintf(send_data, sizeof(send_data), "%f", g_Vsys_min);
}
else if (strcmp(cmd_ar,"get_Vbat") == 0){
snprintf(send_data, sizeof(send_data), "%f", g_Vbat);
}
else if (strcmp(cmd_ar,"get_Vbat_max") == 0){
snprintf(send_data, sizeof(send_data), "%f", g_Vbat_max);
}
else if (strcmp(cmd_ar,"get_Vbat_min") == 0){
snprintf(send_data, sizeof(send_data), "%f", g_Vbat_min);
}
else if (strcmp(cmd_ar,"set_drive_pwm_clkdiv") == 0){
g_drive_pwm_clkdiv = atoi(cmd_data_ar);
uint slice_num_drive = pwm_gpio_to_slice_num(GPIO_DRIVE_PWM);
pwm_set_clkdiv(slice_num_drive, g_drive_pwm_clkdiv);
snprintf(send_data, sizeof(send_data), "%d", g_drive_pwm_clkdiv);
}
else if (strcmp(cmd_ar,"set_drive_pwm_warp") == 0){
g_drive_pwm_warp = atoi(cmd_data_ar);
snprintf(send_data, sizeof(send_data), "%d", g_drive_pwm_warp);
}
else if (strcmp(cmd_ar,"enable_break") == 0 ){
if (atoi(cmd_data_ar) == 1){
g_enable_break = 1;
}else{
g_enable_break = 0;
}
snprintf(send_data, sizeof(send_data), "%d", g_enable_break);
}
else{
return ERROR;
}
uart_puts(UART_ID,cmd_ar);
uart_puts(UART_ID,",");
uart_puts(UART_ID,send_data);
uart_puts(UART_ID,"\n");
return NON_ERROR;
}
int uart_process(char uart_rxdata_arr[], char cmd_ar[], char cmd_data_ar[]){
int i = 0;
while (uart_rxdata_arr[i] != ','){
cmd_ar[i] = uart_rxdata_arr[i];
i++;
if (i > 20){
chars_rxed = 0;
return ERROR;
}
}
cmd_ar[i] = '\0';
i++;
int command_len = i;
while (uart_rxdata_arr[i] != '\n'){
cmd_data_ar[i-command_len] = uart_rxdata_arr[i];
i++;
if (i > 40){
chars_rxed = 0;
return ERROR;
}
}
cmd_data_ar[i-command_len] = '\0';
chars_rxed = 0;
return NON_ERROR;
}
void master_off(){
g_drive_rps = 0;
setup_drive_motor();
setup_servo_motor();
}
int chk_condition(void){
g_Vbat = get_batt();
g_Vsys = get_vsys();
g_temp = get_temp();
if (g_Vbat_max < g_Vbat){
g_Vbat_max = g_Vbat;
}
if (g_Vbat_min > g_Vbat){
g_Vbat_min = g_Vbat;
}
if (g_Vsys_max < g_Vsys){
g_Vsys_max = g_Vsys;
}
if (g_Vsys_min > g_Vsys){
g_Vsys_min = g_Vsys;
}
if (g_temp_max < g_temp){
g_temp_max = g_temp;
}
if (g_temp_min > g_temp){
g_temp_min = g_temp;
}
if(g_Vbat_max >= 14){
return ERROR;
}
if (g_Vbat_min < 7){
return ERROR;
}
if (g_Vsys_max > 5.5){
return ERROR;
}
if (g_Vsys_min < 4.5){
return ERROR;
}
if (g_temp_max > 50){
return ERROR;
}
if (g_temp_min < 0){
return ERROR;
}
return NON_ERROR;
}
void core1_main(void){
setup_enc();
setup_drive_motor();
gpio_set_irq_enabled_with_callback(GPIO_ENC_A, GPIO_IRQ_EDGE_RISE, true, enc_callback);
gpio_set_irq_enabled_with_callback(GPIO_ENC_A, GPIO_IRQ_EDGE_FALL, true, enc_callback);
gpio_set_irq_enabled_with_callback(GPIO_ENC_B, GPIO_IRQ_EDGE_RISE, true, enc_callback);
gpio_set_irq_enabled_with_callback(GPIO_ENC_B, GPIO_IRQ_EDGE_FALL, true, enc_callback);
static repeating_timer timer;
add_repeating_timer_ms(-10, repeating_timer_callback, NULL, &timer);
}
int main(void) {
stdio_init_all();
multicore_launch_core1(core1_main);
setup_servo_motor();
setup_dist_wave();
setup_adc();
setup_uart();
gpio_init(GPIO_PICO_LED);
gpio_set_dir(GPIO_PICO_LED, GPIO_OUT);
gpio_put(GPIO_PICO_LED, 0);
char cmd_ar[20] = {};
char cmd_data_ar[20] = {};
uint led_flash = 500;
uint32_t led_on_time=0,led_off_time=to_ms_since_boot(get_absolute_time());
sleep_ms(1000);
g_Vbat_max = get_batt();
g_Vbat_min = get_batt();
g_Vsys_max = get_vsys();
g_Vsys_min = get_vsys();
g_temp_max = get_temp();
g_temp_min = get_temp();
while (true) {
if (gpio_get(GPIO_PICO_LED) == 0 && to_ms_since_boot(get_absolute_time()) - led_off_time >= led_flash){
led_on_time = to_ms_since_boot(get_absolute_time());
gpio_put(GPIO_PICO_LED,1);
}
if (gpio_get(GPIO_PICO_LED) == 1 && to_ms_since_boot(get_absolute_time()) - led_on_time >= led_flash){
gpio_put(GPIO_PICO_LED,0);
led_off_time = to_ms_since_boot(get_absolute_time());
}
if (chk_condition() == ERROR){
master_off();
led_flash = 100;
g_total_fault = 1;
}
if (uart_flg == 1){
uart_flg = 0;
if (uart_process(uart_rx_data,cmd_ar, cmd_data_ar) == NON_ERROR){
if (command_processing(cmd_ar,cmd_data_ar) == ERROR){
UART_SEND_ERR_MESSAGE
}
}else{
UART_SEND_ERR_MESSAGE
}
}
}
return 0;
}
