104 lines
2.6 KiB
C
104 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);
|
||
|
|
}
|