2024-01-13 12:57:51 -05:00
|
|
|
#include "debug.h"
|
|
|
|
#include "can.h"
|
|
|
|
#include "ws2812.h"
|
|
|
|
#include "memecoder.h"
|
|
|
|
#include "ctre.h"
|
|
|
|
#include "mode.h"
|
|
|
|
|
|
|
|
#include <string.h>
|
|
|
|
|
|
|
|
|
|
|
|
void init_switch_gpio() {
|
|
|
|
// Enable GPIOB
|
|
|
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
|
2024-01-26 16:04:31 -05:00
|
|
|
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
|
|
|
|
|
2024-01-13 12:57:51 -05:00
|
|
|
|
|
|
|
// Set up switch on PB5 and PB6, use internal pull ups
|
|
|
|
GPIO_InitTypeDef GPIO_InitStructure;
|
|
|
|
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
|
|
|
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
|
|
|
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
|
|
|
|
GPIO_Init(GPIOB, &GPIO_InitStructure);
|
|
|
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
|
|
|
|
GPIO_Init(GPIOB, &GPIO_InitStructure);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
u8 get_switch_state() {
|
|
|
|
u8 state = 0;
|
|
|
|
state |= (!GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_5)) << 0;
|
|
|
|
state |= (!GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_6)) << 1;
|
|
|
|
return state;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
int main(void)
|
|
|
|
{
|
|
|
|
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
|
|
|
|
Delay_Init();
|
|
|
|
USART_Printf_Init(115200);
|
|
|
|
printf("SystemClk:%d\r\n", SystemCoreClock);
|
|
|
|
|
|
|
|
// Inititalize hardware
|
|
|
|
init_switch_gpio();
|
|
|
|
memecoder_init();
|
|
|
|
can_init();
|
|
|
|
ctre_init();
|
|
|
|
// ws2812_init();
|
|
|
|
|
|
|
|
// Clear all existing ports
|
|
|
|
ctre_port_reset_all();
|
|
|
|
|
|
|
|
// Main loop
|
|
|
|
u8 magic[6];
|
|
|
|
|
|
|
|
u8 devices[8];
|
|
|
|
u8 dev_cnt = 0;
|
|
|
|
|
|
|
|
CanRxMsg rx_msg;
|
|
|
|
|
|
|
|
CanTxMsg tx_msg;
|
|
|
|
tx_msg.IDE = CAN_Id_Extended;
|
|
|
|
tx_msg.RTR = CAN_RTR_Data;
|
|
|
|
tx_msg.DLC = 0;
|
|
|
|
|
|
|
|
app_state_t app;
|
|
|
|
app.device = 0;
|
|
|
|
app.switch_state = 0;
|
|
|
|
|
|
|
|
mode_openloop_state_t openloop_state;
|
|
|
|
mode_openloop_init(&app, &openloop_state);
|
|
|
|
|
|
|
|
mode_position_state_t position_state;
|
|
|
|
mode_position_init(&app, &position_state);
|
|
|
|
|
|
|
|
mode_position2_state_t position2_state;
|
|
|
|
mode_position2_init(&app, &position2_state);
|
|
|
|
while(1)
|
|
|
|
{
|
|
|
|
memecoder_update();
|
|
|
|
app.switch_state = get_switch_state();
|
|
|
|
|
|
|
|
for(int i = 0; i < dev_cnt; i++) {
|
|
|
|
app.device = devices[i];
|
|
|
|
printf("Device: %d, ss: %d\r\n", app.device, app.switch_state);
|
|
|
|
mode_openloop(&app, &openloop_state);
|
|
|
|
// mode_position(&app, &position_state);
|
|
|
|
// mode_position2(&app, &position2_state);
|
|
|
|
}
|
|
|
|
|
2024-01-26 16:04:31 -05:00
|
|
|
printf("%d\r\n", memecoder_get());
|
|
|
|
|
2024-01-13 12:57:51 -05:00
|
|
|
// Read data from CAN
|
|
|
|
if(can_recv(&rx_msg)) {
|
|
|
|
u32 arbid = rx_msg.ExtId;
|
|
|
|
if(arbid & CTRE_TALON_SRX) {
|
|
|
|
// Talon waiting to be adopted, is sending magic codes
|
|
|
|
if((arbid & 0xFFFFFF80) == 0x0204F800) {
|
|
|
|
printf("ASK ADOPT!\r\n");
|
|
|
|
// Save magic for later
|
|
|
|
memcpy(magic, rx_msg.Data, 6);
|
|
|
|
|
|
|
|
// Request tell it we want to be on channel 6
|
|
|
|
tx_msg.ExtId = 0x0004F87F;
|
|
|
|
tx_msg.DLC = 8;
|
|
|
|
tx_msg.Data[0] = 0; tx_msg.Data[1] = 0; tx_msg.Data[2] = 0; tx_msg.Data[3] = 0;
|
|
|
|
tx_msg.Data[4] = 0; tx_msg.Data[5] = 0; tx_msg.Data[6] = 0; tx_msg.Data[7] = ctre_port;
|
|
|
|
CAN_Transmit(CAN1, &tx_msg);
|
|
|
|
}
|
|
|
|
|
|
|
|
// We got a response to our request to be an channel 6
|
|
|
|
if((arbid & 0xFFFFFF80) == 0x0204F880) {
|
|
|
|
printf("ADOPT!\r\n");
|
|
|
|
tx_msg.ExtId = 0x0004F8FF;
|
|
|
|
tx_msg.DLC = 8;
|
|
|
|
memcpy(tx_msg.Data, magic, 6);
|
|
|
|
tx_msg.Data[6] = 0; tx_msg.Data[7] = ctre_port;
|
|
|
|
CAN_Transmit(CAN1, &tx_msg);
|
|
|
|
ctre_port++;
|
|
|
|
|
|
|
|
printf("Magic: %02X %02X %02X %02X %02X %02X\r\n", magic[0], magic[1], magic[2], magic[3], magic[4], magic[5]);
|
|
|
|
|
|
|
|
// app.device = arbid & 0x1F;
|
|
|
|
devices[dev_cnt++] = arbid & 0x1F;
|
|
|
|
|
|
|
|
// Factory reset
|
|
|
|
ctre_factory_reset(app.device);
|
|
|
|
|
|
|
|
Delay_Ms(100);
|
|
|
|
|
|
|
|
|
|
|
|
// Polarity
|
|
|
|
ctre_set_param(app.device, 337, 1, 0, 0);
|
|
|
|
|
|
|
|
// Sensor
|
|
|
|
// ctre_set_param(app.device, 330, 0, 0, 0);
|
|
|
|
Delay_Ms(10);
|
|
|
|
ctre_set_param(app.device, 330, 1, 0, 0);
|
|
|
|
// P
|
|
|
|
// ctre_set_param(app.device, 310, 0x400000, 0, 0);
|
|
|
|
Delay_Ms(10);
|
|
|
|
ctre_set_param(app.device, 310, (uint32_t)(((float)0x400000) * 0.04), 0, 0);
|
|
|
|
Delay_Ms(10);
|
|
|
|
ctre_set_param(app.device, 311, (uint32_t)(((float)0x400000) * 0.000005), 0, 0);
|
|
|
|
Delay_Ms(10);
|
|
|
|
ctre_set_param(app.device, 312, (uint32_t)(((float)0x400000) * 0.0), 0, 0);
|
|
|
|
Delay_Ms(10);
|
|
|
|
ctre_set_param(app.device, 313, (uint32_t)(((float)0x400000) * 0.0), 0, 0);
|
|
|
|
Delay_Ms(10);
|
|
|
|
ctre_set_param(app.device, 317, 1023, 0, 0);
|
|
|
|
// I
|
|
|
|
// ctre_set_param(app.device, 311, 0x40000, 0, 0);
|
|
|
|
|
|
|
|
// IZone
|
|
|
|
// ctre_set_param(app.device, 314, 0, 0, 0);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
Delay_Ms(1);
|
|
|
|
}
|
|
|
|
}
|