talon/User/ctre.c
2024-01-13 12:57:51 -05:00

103 lines
2.6 KiB
C

#include "ctre.h"
int ctre_port;
void ctre_init() {
ctre_port = 6;
}
void ctre_feed_enable(u8 enable) {
CanTxMsg TxMessage;
TxMessage.ExtId = CTRE_ENABLE;
TxMessage.IDE = CAN_Id_Extended;
TxMessage.RTR = CAN_RTR_Data;
TxMessage.DLC = 8;
TxMessage.Data[0] = enable;
TxMessage.Data[1] = 0x00;
TxMessage.Data[2] = 0x00;
TxMessage.Data[3] = 0x00;
TxMessage.Data[4] = 0x00;
TxMessage.Data[5] = 0x00;
TxMessage.Data[6] = 0x00;
TxMessage.Data[7] = 0x00;
CAN_Transmit(CAN1, &TxMessage);
}
void ctre_percent_out(u8 id, double cmd) {
int cmdi = cmd * 1023;
if(cmdi > 1023) cmdi = 1023;
if(cmdi < -1023) cmdi = -1023;
CanTxMsg TxMessage;
TxMessage.ExtId = CTRE_TALON_SRX | CTRE_CONTROL(3) | id;
TxMessage.IDE = CAN_Id_Extended;
TxMessage.RTR = CAN_RTR_Data;
TxMessage.DLC = 8;
TxMessage.Data[0] = cmdi >> 16;
TxMessage.Data[1] = cmdi >> 8;
TxMessage.Data[2] = cmdi;
TxMessage.Data[3] = 0x00;
TxMessage.Data[4] = 0x00;
TxMessage.Data[5] = 0x00;
TxMessage.Data[6] = 0x00;
TxMessage.Data[7] = 0x00;
CAN_Transmit(CAN1, &TxMessage);
}
void ctre_position_out(u8 id, int pos) {
CanTxMsg TxMessage;
TxMessage.ExtId = CTRE_TALON_SRX | CTRE_CONTROL(3) | id;
TxMessage.IDE = CAN_Id_Extended;
TxMessage.RTR = CAN_RTR_Data;
TxMessage.DLC = 8;
TxMessage.Data[0] = pos >> 16;
TxMessage.Data[1] = pos >> 8;
TxMessage.Data[2] = pos;
TxMessage.Data[3] = 0x00;
TxMessage.Data[4] = 0x00;
TxMessage.Data[5] = 0x01; // Invert?
TxMessage.Data[6] = 0x00;
TxMessage.Data[7] = 0x80; // 80
CAN_Transmit(CAN1, &TxMessage);
}
void ctre_set_param(u8 id, u16 param_idx, u32 value, u8 sub_value, u32 ordinal) {
u8 param_idx_h8 = param_idx >> 4;
u8 param_idx_l4 = param_idx & 0xF;
CanTxMsg TxMessage;
TxMessage.ExtId = CTRE_TALON_SRX | CTRE_PARAM_SET | id;
TxMessage.IDE = CAN_Id_Extended;
TxMessage.RTR = CAN_RTR_Data;
TxMessage.DLC = 8;
TxMessage.Data[0] = param_idx_h8;
TxMessage.Data[1] = ordinal | (param_idx_l4 << 4);
TxMessage.Data[2] = value >> 0x18;
TxMessage.Data[3] = value >> 0x10;
TxMessage.Data[4] = value >> 0x08;
TxMessage.Data[5] = value;
TxMessage.Data[6] = 0x00;
TxMessage.Data[7] = sub_value;
CAN_Transmit(CAN1, &TxMessage);
}
void ctre_factory_reset(u8 port) {
CanTxMsg tx_msg;
tx_msg.ExtId = CTRE_TALON_SRX | CTRE_FACTORY_RESET | port;
tx_msg.IDE = CAN_Id_Extended;
tx_msg.RTR = CAN_RTR_Data;
tx_msg.DLC = 3;
tx_msg.Data[0] = 0x02;
tx_msg.Data[1] = 0xC5;
tx_msg.Data[2] = 0x01;
CAN_Transmit(CAN1, &tx_msg);
}
void ctre_port_reset_all() {
CanTxMsg tx_msg;
tx_msg.ExtId = CTRE_RESET_PORTS;
tx_msg.IDE = CAN_Id_Extended;
tx_msg.RTR = CAN_RTR_Data;
tx_msg.DLC = 0;
CAN_Transmit(CAN1, &tx_msg);
}