void initServos()
{
Timer4Init(20000);
Timer3Init(20000);
Timer12Init(20000);
PWM12Ch1Init(AngleToDuty(0.0, HD_1501MG)); // servo przy podstawie (obrót wokó³ osi pionowej)
PWM12Ch2Init(AngleToDuty(0.0, HD_1501MG)); // servo przy podstawie (obrót wokó³ osi poziomej)
PWM3Ch1Init(AngleToDuty(90.0, HD_1201MG)); // serwo na przegubie 3
PWM3Ch2Init(AngleToDuty(0.0, HD_1201MG)); // servo na przegubie 4
PWM3Ch3Init(AngleToDuty(0.0, HS_645MG)); // servo na przegubie 5 (oœ wzd³u¿ ogniwa)
PWM4Ch1Init(AngleToDuty(0.0, HS_485HB)); // servo na przegubie 6
PWM4Ch2Init(AngleToDuty(45.0, HS_55)); // chwytak 1
PWM4Ch3Init(AngleToDuty(-45.0, HS_55)); // chwytak 2
}
void HilNodeInit(void)
{
// Set a unique node ID for this node.
nodeId = CAN_NODE_HIL;
// And configure the Peripheral Pin Select pins:
PPSUnLock;
// To enable ECAN1 pins: TX on 7, RX on 4
PPSOutput(OUT_FN_PPS_C1TX, OUT_PIN_PPS_RP7);
PPSInput(PPS_C1RX, PPS_RP4);
// To enable UART1 pins: TX on 11, RX on 13
PPSOutput(OUT_FN_PPS_U1TX, OUT_PIN_PPS_RP11);
PPSInput(PPS_U1RX, PPS_RP13);
// Configure SPI1 so that:
// * (input) SPI1.SDI = B8
PPSInput(PPS_SDI1, PPS_RP10);
// * SPI1.SCK is output on B9
PPSOutput(OUT_FN_PPS_SCK1, OUT_PIN_PPS_RP9);
// * (output) SPI1.SDO = B10
PPSOutput(OUT_FN_PPS_SDO1, OUT_PIN_PPS_RP8);
PPSLock;
// Enable pin A4, the amber LED on the CAN node, as an output. We'll blink this at 1Hz. It'll
// stay lit when in HIL mode with it turning off whenever packets are received.
_TRISA4 = 0;
// Initialize communications for HIL.
HilInit();
// Set Timer4 to be a 4Hz timer. Used for blinking the amber status LED.
Timer4Init(HilNodeBlink, 39062);
// Set up Timer2 for a 100Hz timer. This triggers CAN message transmission at the same frequency
// that the sensors actually do onboard the boat.
Timer2Init(HilNodeTimer100Hz, 1562);
// Initialize ECAN1
Ecan1Init();
// Set a schedule for outgoing CAN messages
// Transmit the rudder angle at 10Hz
if (!AddMessageRepeating(&sched, SCHED_ID_RUDDER_ANGLE, 10)) {
FATAL_ERROR();
}
// Transmit the rudder status at 10Hz
if (!AddMessageRepeating(&sched, SCHED_ID_RUDDER_LIMITS, 10)) {
FATAL_ERROR();
}
// Transmit the throttle status at 100Hz
if (!AddMessageRepeating(&sched, SCHED_ID_THROTTLE_STATUS, 10)) {
FATAL_ERROR();
}
// Transmit the RC status at 2Hz
if (!AddMessageRepeating(&sched, SCHED_ID_RC_STATUS, 2)) {
FATAL_ERROR();
}
// Transmit latitude/longitude at 5Hz
if (!AddMessageRepeating(&sched, SCHED_ID_LAT_LON, 5)) {
FATAL_ERROR();
}
// Transmit heading & speed at 5Hz
if (!AddMessageRepeating(&sched, SCHED_ID_COG_SOG, 5)) {
FATAL_ERROR();
}
// Transmit heading & speed at 5Hz
if (!AddMessageRepeating(&sched, SCHED_ID_GPS_FIX, 5)) {
FATAL_ERROR();
}
}