skarven
Joined: 12 Mar 2009
Posts: 2
|
| Boat Autopilot on 16F876 |
 Posted: Fri Mar 13, 2009 12:20 pm |
 |
|
Hi
A few years ago I made an autopilot for my boat on a 16F876.
It has been used for about 5000 hours of boating and has one known bug.
(not serious!)
It controls a hydraulic valve with two solenoids and uses an electronic
compass that sends 10 $HEHDT telegrams/s. This is the system clock.
Input to the program is an infrared Sony remote control, and I think this part
of the code could be interesting to some. It might not be very elegant, but
it works very well.
Output is to a LCD 20*4 display.
The Autopilot is special in that it does not use a rudder position sensor, and while "normal" autopilots run the rudder from port to starboard in about 8s, this one does it in about 2s.
It will control the 25 feet boat in 8 feet waves! Tested even in following seas!
The LCD interface is CCS modified to free B0 pin for IR interface
This autopilot has given us thousands of hours of relaxed boating in a
relatively course unstable boat.
Kai Dahlqvist
[email protected]
[email protected]
| Code: |
// Autopilot
//
// This program is a complete Autopilot for my boat which is a
// Nord West 741 with length 7.4m beam 2.8m. The engine is a
// Volvo Penta MD40A giving 85HP and a maximum cruising speed of about
// 13 knots.
//
// The steering system is Tennfjord
// with two hand pumps with capacity app. 27ccm/revolution.
// Steering cylinder is 96 ccm.
//
// The Servo system is A Servi pump with capacity app. 2 liters/min
// Control of the servo system is by two digital outputs from the PIC controlling
// the hydraulic valve solenoids
// In addition there is a filter change warning signal input, alarm output
// and a Windscreen Viper output.
//
// Heading sensor is Furuno PG-1000 set up to give 10 HCHDT telegrams every second.
//
// The user interface is an LCD display and all user input to the program is by
// a SONY remote control. The remote control signal is input to Pin B0 from
// an IRM-8601S infrared receiver and decoded with interrupt on change B0.
//
// The backlight for the LCD is controlled by Pulse Width
// Modulation from the CCP1.
//
//#include <stdlib.h>
//#define DIAG // only for testing and debugging
#define DPRINT6
//#include <\picc\include\16F877.H> // 40-pin device
//#include <\picc\include\16F876.H> // 28-pin device
//#include <\picc\include\inc16f876.h> // 28-pin device
#include "c:\picc\include\16F876.H" // 28-pin device
#include "c:\picc\include\stdlib.h"
//#include "c:\picc\include\STDLIB.H"
#fuses HS,NOWDT,NOPROTECT,PUT,NOBROWNOUT,NOLVP // May need to be changed depending on the chip
#use delay(clock=20000000) // Change if you use a different clock
#use rs232(baud=4800, xmit=PIN_C6, rcv=PIN_C7)
////////////////////////////////////////////////////////////////////////////
//// LCD.C ////
//// Driver for common LCD modules ////
//// ////
//// lcd_init() Must be called before any other function. ////
//// ////
//// lcd_putc(c) Will display c on the next position of the LCD. ////
//// The following have special meaning: ////
//// \f Clear display ////
//// \n Go to start of second line ////
//// \b Move back one position ////
//// ////
//// lcd_gotoxy(x,y) Set write position on LCD (upper left is 1,1) ////
//// ////
//// lcd_getc(x,y) Returns character at position x,y on LCD ////
//// ////
//// LCDB0I.C is a modified LCD.C which frees the B0 pin for input ////
////////////////////////////////////////////////////////////////////////////
#include "c:\picc\kpilot\lcdb0i.c"
//#define T1_1 85 Theoretical
//#define T1_65 129
//#define T2_2 171
#define T1_1 93 // These values were set after testing the real thing
#define T1_65 139 //
#define T2_2 186 // 184 - 191
#define MErr 10
#define FILTER_COMPASS // Filter compass input
#define MAXSPEED (12) // Boats maximum speed
#define MAXRUDDER (750) // Maximum number for rudder angle
#priority rtcc,ext
//---------------------------------------------------------------------------
// Global variables for Timer 2
//
signed long sl2msTimer = 0; // incremented every 2 ms
short int bCompassAlarm = 0; // Will give short beep in alarm if compass not received in 1 second
short int bBlinkDisplay = 0; // Will blink the LCD backlight
short int bFilterAlarm = 0; // Indicates that filter alarm has been given
short int bToggleLight = 0; // Used to Blink LCD backlight
char cError = 0; // Indicates which error to display
// 0 = OK, 1 = Compass Error, 2 = Late Button, 3 = Filter Change
//---------------------------------------------------------------------------
// Global variables for PID - regulator
//
signed long slHeading = 0;
signed long slOldHeading;
#ifdef FILTER_COMPASS
signed long slNewHeading;
signed long slDiff;
#endif
signed long slHeadingError;
signed long slHeadingIntegral = 0;
signed long slSetPoint = 0;
signed long slROT;
signed long slROTFilt;
char cCount10;
//signed scSetDelayPort; // delay to allow valve time to open
//signed scSetDelayStbd; // delay to allow valve time to open
char cStartDelayPort; // delay to allow valve time to open
char cStartDelayStbd; // delay to allow valve time to open
short int bLastMoveStbd; // Indicates last move was to starboard
signed long slRud = 0; // Temporary variable
signed long slRudder = 0; // Actual Rudder Angle
signed long slCalcRudder = 0; // Calculated partly from the ROT
signed long slOldRudder = 0; // Actual rudder angle last second
signed long slSetRudder = 0; // Rudder setpoint
signed long slRudderMove = 0;
signed long slRudFromROTCalc;
float fRudder;
short int bAutopilot = 0;
short int bAlarmEnable = 1;
#define pPumpPort PIN_A2 // RA2 (Pin4) Port Pump Solenoid Ouput Pin
#define pPumpStbd PIN_C3 // RC0 (Pin11) Stbd Pump Solenoid Ouput Pin
#define pAlarm PIN_C4 // RC1 (Pin12) Alarm Output Pin
#define pViper PIN_C5 // Viper control Output
#define pFilter PIN_A4 // Filter Change Input
char cViperPeriod = 0; // Time between viper runs, 0=stop,1=continious
char cViperCnt; // Count seconds since last viper run
short int bViperOnce = 0; // To run viper by pressing D. Keystone
char cIdx = 0;
float fVar[8];
const char c1[8] = { 'K','K','K','S','K','x','x','S' };
const char c2[8] = { 'p','i','d','p','2','x','x','c' };
//---------------------------------------------------------------------------
// Global variables for serial receive
//
#define BUFFER_SIZE 8 // 6 is enough, but...
char cBuffer[BUFFER_SIZE];
char cNextIn = 0;
char cNMEAMode = 0;
char cCheckSum;
char cACheckSum;
short bCompassReceived = 0; // Indicates compass message received
short bFirstCompass = 0; // Indicate first compass telegram received
char cCompassReceive = 0; // Cycle counter for compass receive (to indicate that messages are received)
//---------------------------------------------------------------------------
// Global variables for remote control receive
//
#ifdef DIAG
char c1_1 = 0;
char c1_65 = 0;
char c2_2 = 0;
char cX_X = 0;
#endif
char cSameCnt = 0; // Same command received X times
char cIdleCnt = 0; // No command received for X * 3.28ms
char cStartType; //= 0; // Indicate length of the header pulse 1 = 1.1us, 2 = 2.2us
char cStatus; //= 0; // 0 is OK
char cBitCnt; //= 0; // Number of bits received after Start pulse
char cBits[3]; //= { 1,2,3 }; // Remote control command input ends up here
char cLast1StartType; // = 0;
char cLast1Status; // = 0;
char cLast1BitCnt; // = 0;
char cLast1Bits[3];
#ifdef DIAG
char cLast2StartType = 0;
char cLast2Status = 0;
char cLast2BitCnt = 0;
char cLast2Bits[3];
#endif
char cPulseState = 0; // State for remote control state machine
short bBit; // used to hold data bit
short bCmdReceived = 0; // Indicates command to be executed
char cCmdBits; // only 8 of the bits in a command is used
char cCmdBitCnt; // number of bits in received remote control command
char j;
#ifdef DIAG
char k;
#endif
//---------------------------------------------------------------------------
// Variables for Menu system
//
char cMenuMode = 0; // 0 = Main menu, 1 = Config menu, 2 = diagnostic menu
char cLight = 15; // LCD backlight intensity
//---------------------------------------------------------------------------
// Interrupt for serial input for telegram: $HCHDT,XXX.X,T*CC<CR>,<LF>
//---------------------------------------------------------------------------
#INT_RDA
void serial_isr() {
int t;
t=getc();
// if (cNMEAMode < 5) cCheckSum ^= t;
switch (cNMEAMode) {
case 0: // wait for $
if (t == '$') {
cNMEAMode = 1;
cNextIn = 0;
cCheckSum = 0;
}
break;
case 1: // skip heading, wait for ','
cCheckSum ^= t;
if (t == ',') cNMEAMode = 2; // input number next
break;
case 2: // input integer and decimal part, skip '.', wait for ','
cCheckSum ^= t;
if (t == ',') {
cBuffer[cNextIn] = 0; // terminate string
cNMEAMode = 3; // finnished with number
} else {
if (t != '.') {
cBuffer[cNextIn] = t;
cNextIn += 1;
}
}
break;
case 3: // wait for '*'
if (t == '*') {
cNMEAMode = 4; // Go get checksum
} else {
cCheckSum ^= t; // * is not included in checksum
}
break;
case 4: // get first checksum byte
if (t > '9') cACheckSum = t - 55;
else cACheckSum = t - 48;
swap(cACheckSum);
cNMEAMode = 5;
break;
case 5: // get second checksum byte
if (t > '9') t = t - 55;
else t = t - 48;
cACheckSum = cACheckSum | t;
if (cAChecksum == cChecksum) {
bCompassReceived = 1; // Signal Main Loop that compass has been received
sl2msTimer = 0; // Clear compass timeout for timer 2
}
cNMEAMode = 0;
break;
}
}
//---------------------------------------------------------------------------
// Timer 2 interrupt every 4ms (3.4ms on the scope)
// This interrupt is used to time the pulses to the hydraulic valve solenoids
// Execution app. 10 us.
//---------------------------------------------------------------------------
#INT_TIMER2
TIMER2_Interrupt()
{
signed long slTmp;
slTmp = slSetRudder - slRudder;
if (slTmp) { // do we have to move the rudder
if (slTmp > 0) { // Move rudder starboard ?
bLastMoveStbd = 1; // Indicate last move was to stbd
if (cStartDelayStbd) {
cStartDelayStbd = cStartDelayStbd - 1; // Allow valve time to open
} else {
slRudder = slRudder + 1; // Increment Actual Rudder Angle
}
OUTPUT_LOW(pPumpPort); // Make shure both bits not set
OUTPUT_HIGH(pPumpStbd); // Set Stbd Output Pin High
} else { // Move rudder port
bLastMoveStbd = 0; // Indicate last move was to port
if (cStartDelayPort) {
cStartDelayPort = cStartDelayPort - 1; // Allow valve time to open
} else {
slRudder = slRudder - 1; // Decrement Actual Rudder Angle
}
OUTPUT_LOW(pPumpStbd); // Make shure both bits not set
OUTPUT_HIGH(pPumpPort); // Set Port Output Pin High
}
} else { // if (slTmp) { // The rudder position is correct
OUTPUT_LOW(pPumpPort); // Clear both Hydraulic Pump bits
OUTPUT_LOW(pPumpStbd); // Don't care which one was set
if (bLastMoveStbd) {
cStartDelayStbd = 18; // scSetDelayStbd; // Normal start delay for continued Stbd movement
cStartDelayPort = 17 + 3; // scSetDelayPort; // Increase start delay to let walve move the extra distance
} else {
cStartDelayStbd = 18 + 3; // scSetDelayStbd; // Increase start delay to let walve move the extra distance
cStartDelayPort = 17; // scSetDelayPort; // Normal start delay for continued port movement
}
}
sl2msTimer = sl2msTimer + 1; // sl2msTimer incremented every interrupt
if (sl2msTimer > 740) {
bCompassAlarm = 1;
sl2msTimer = 0;
}
}
//---------------------------------------------------------------------------
// Interrupt when Timer 0 overflows from 0xFF to 0x00
// Timer 0 interrupt is together with B0 interrupt used to decode the input
// from the remote control. Pulse lengths is timed with Timer 0, and the end of
// a command is detected by Timer 0 overflowing.
//
// if the cPulseState is non-zero this interrupt comes 3.28ms after the last
// pulse on B0. This is the normal way the end of a command from the remote
// control is detected.
//
// If cPulseState is zero, that indicates that we are not receiving any commands.
// This will happen every 3.28ms when no commands are received.
//
//---------------------------------------------------------------------------
#INT_RTCC
RTCC_Interrupt()
{
disable_interrupts(INT_EXT); // Disable PIN B0 interrupt
ext_int_edge(H_TO_L); // Set to look for start of next Start Pulse
if (cPulseState) { // are we receiving a command that timed out
#ifdef DIAG
cLast2StartType = cLast1StartType;
cLast2BitCnt = cLast1BitCnt;
cLast2Status = cLast1Status;
cLast2Bits[0] = cLast1Bits[0];
cLast2Bits[1] = cLast1Bits[1];
cLast2Bits[2] = cLast1Bits[2];
#endif
// Shift command bits right so they go from bit 0 to cBitCnt-1
j = cBitCnt;
while (j < 24) { // Get Bits to the Right
SHIFT_RIGHT(cBits, 3, 0);
j++;
}
// Check if the same command is received again. The remote control will
// send commands as long as the button is pressed.
if ((cStartType==cLast1StartType) && (cBitCnt==cLast1BitCnt) &&
(cStatus==0) && (cBits[0]==cLast1Bits[0]))
{ // Same command
cSameCnt++; // Count it
if (cSameCnt==2) {
bCmdReceived = 1; // Set Command received bit if 2 equal cmds received
cCmdBits = cLast1Bits[0];
cCmdBitCnt = cLast1BitCnt;
} else {
// if (cSameCnt>20) { // Button held for 0.7sec. (a bit slow
// if (cSameCnt>16) { // Button held for 0.7sec.
if (cSameCnt>12) { // Button held for 0.7sec.
// cSameCnt = 17; // Repeat command in a short time
// cSameCnt = 13; // Repeat command in a short time
cSameCnt = 9; // Repeat command in a short time
bCmdReceived = 1; // Set Command received bit to repeat command
}
}
} else { // New command or bad status
cSameCnt = 0;
cLast1StartType = cStartType; // Transfer bits,cnt,type,status to "Last1"
cLast1BitCnt = cBitCnt;
cLast1Status = cStatus;
cLast1Bits[0] = cBits[0];
cLast1Bits[1] = cBits[1];
cLast1Bits[2] = cBits[2];
}
cStartType = 0; // Undefined pulse yet.
cBitCnt = 0;
cStatus = 0;
cBits[0] = 0;
cBits[1] = 0;
cBits[2] = 0;
cPulseState = 0; // Set state to wait for start of new start pulse
#ifdef DIAG
k++;
#endif
} else { // if (cPulseState) { // are we receiving a command that timed out, No this is idle
if (cIdleCnt < 250) cIdleCnt++; // don't overflow idle count
if (cIdleCnt == 10) { // if idle for 30ms, reset last to enable new command
cSameCnt = 0;
cLast1StartType = 0; // Set Last
cLast1BitCnt = 0;
cLast1Status = 0;
cLast1Bits[0] = 0;
// cLast1Bits[1] = 0;
// cLast1Bits[2] = 0;
}
} // if (cPulseState) {
enable_interrupts(INT_EXT); // Enable PIN B0 interrupt
}
//---------------------------------------------------------------------------
// Interrupt when PIN B0 changes state
//
// This interrupt decodes the remote control input on B0.
// Note that the timing of the start pulse is from the leading H to L
// transition to the L to H one.
// The normal data pulses is timed from the L to H to the L to H on the next pulse.
//
//
// Start Pulse
//--------------- ------- -------- ----.....
// | 2.2us | | 0.55us | | 0.55 |
// | or |0.55us | or | 0.55us | or |
// | 1.1us | | 1.1us | | 1.1us |
// ------------------------ ---..--- ---..---
//
//B0 Interrupt 1 2 3 4
//
//
// The length of the start pulse is indicated by the cStartType variable.
// Some commands from SONY remotes have a start pulse of 1.1 us which set sStartType = 1.
// All the commands used in this program has a start pulse of 2.2us and a starttype of 2.
// The remote control that I use is for a SONY projector, and the type number on it is RM-PJ2.
// I also have another one, the RM-PJM10 with the same buttons giving the same codes.
//
// Button Bit Pattern Bits Start Type
// On/Off 002A15 15 2
// Volume + 002A12 15 2
// Volume - 002A13 15 2
// Muting/Pic 002A24 15 2
// INPUT 002A57 15 2
// MENU 002A29 15 2
// ENTER 002A5A 15 2
// RESET 002A7B 15 2
// Right Arrow 002A33 15 2
// Left Arrow 002A34 15 2
// Up Arrow 002A35 15 2
// Down Arrow 002A36 15 2
//
// D Zoom + 02AD6A 20 2
// D Zoom - 02AD6B 20 2
// FREEZE 02AD67 20 2
// APA 02AD60 20 2
// D Keystone 02AD3A 20 2
// Function 1 02AD68 20 2
// Function 2 02AD69 20 2
//---------------------------------------------------------------------------
#INT_EXT
PIN_B0_Interrupt()
{
char cTim;
cTim = get_rtcc();
set_rtcc(0); // Clear timer to check next pulse length
// Clear RTCC interrupt bit also ? Not Necessary
switch (cPulseState) {
case 0: // waiting for Start of Start Pulse
ext_int_edge(L_TO_H); // Set PIN B0 interrupt from Low to High for end of Start Pulse
cIdleCnt = 0; // Reset Idle time indicator
cPulseState = 1; // Set state to wait for end of start pulse
break;
case 1: // waiting for End of Start Pulse
if ((cTim < T1_1-MErr) || (cTim > T2_2+Merr)) { // Pulse was too short (<1.1) or too long (>2.2ms)
#ifdef DIAG
if (cTim < (T1_1-MErr)) cStatus = 1;
else cStatus = 3;
cX_X = cTim; // Save for diagnostic
#else
cStatus = 1;
#endif
cPulseState = 10; // Wait for Inter-code break
} else { // Pulse might be OK
if ((cTim < T2_2-Merr) && (cTim > T1_1+Merr)) { //
cStatus = 2;
#ifdef DIAG
cX_X = cTim; // Save for diagnostic
#endif
cPulseState = 10; // Wait for Inter-code break
} else { // Pulse is OK
if (cTim > T1_1+Merr) { // Pulse 2.2ms
#ifdef DIAG
c2_2 = cTim; // Save for diagnostic
#endif
cStartType = 2; // 2.2ms
cPulseState = 2; // Look for Normal data bits
} else {
#ifdef DIAG
c1_1 = cTim; // Save for diagnostic
#endif
cStartType = 1; // 1.1ms
cPulseState = 10; // Wait for Inter-code break
}
}
}
break;
case 2: // Receiving Normal Data Bits ( 1.1ms or 1.65ms)
if ((cTim < T1_1-MErr) || (cTim > T1_65+Merr)) { // Pulse was too long (>1.65) or too short (<1.1ms)
#ifdef DIAG
if (cTim < (T1_1-MErr)) cStatus = 4;
else cStatus = 6;
cX_X = cTim; // Save for diagnostic
#else
cStatus = 4;
#endif
cPulseState = 10; // Wait for Inter-code break
} else {
if ((cTim < T1_65-Merr) && (cTim > T1_1+Merr)) {
#ifdef DIAG
cX_X = cTim; // Save for diagnostic
#endif
cStatus = 5;
cPulseState = 10; // Wait for Inter-code break
} else { // Pulse is OK
if (cTim > T1_1+Merr) {
#ifdef DIAG
c1_65 = cTim; // Save for diagnostic
#endif
bBit = 1; // 1.65ms
} else {
#ifdef DIAG
c1_1 = cTim; // Save for diagnostic
#endif
bBit = 0; // 1.1ms
}
SHIFT_RIGHT(cBits, 3, bBit);
cBitCnt += 1;
}
}
break;
case 10: // Bit Error. This will just eat bits until end of command
break;
}
}
//---------------------------------------------------------------------------
#SEPARATE
void norm3600(signed long °)
{
if (deg >= 3600) deg = deg - 3600;
if (deg < 0) deg = deg + 3600;
}
//---------------------------------------------------------------------------
#SEPARATE
void norm1800(signed long °)
{
if (deg > 1800) deg = deg - 3600;
if (deg < -1799) deg = deg + 3600;
}
//---------------------------------------------------------------------------
#SEPARATE
void resetall()
{
slHeadingIntegral = 0;
disable_interrupts(INT_TIMER2); // avoid conflict with interrupt routine
slSetRudder = 0; // New Rudder Setpoint
slRudder = 0;
enable_interrupts(INT_TIMER2);
slCalcRudder = 0;
}
//---------------------------------------------------------------------------
#ifdef DPRINT6
#SEPARATE
void print6(signed long l)
{
float f;
if (l < 0) {
lcd_putc('-');
l = - l;
} else {
lcd_putc(' ');
}
f = (l * 0.1) + 0.04;
if (f > 99.95) {
printf(lcd_putc,"%5.1f", f);
} else {
if (f > 9.95) {
printf(lcd_putc," %4.1f", f);
} else {
printf(lcd_putc," %3.1f", f);
}
}
}
#endif
//---------------------------------------------------------------------------
#SEPARATE
void setfromeeprom()
{
// Rudder Angle(Steps) = ROT * (MaxSpeed/Speed) * 0.55 * 10 // for range +/- 375 for rudder angle
// Rudder Angle(Steps) = ROT * (MaxSpeed/Speed) * 0.55 * 20 // for range +/- 750 for rudder angle
// Speed
slRudFromROTCalc = (MAXSPEED/fVar[7]) * 11; // Calculate factor for Rudder from ROT Calculation (MAXRUDDER = 750)
}
//---------------------------------------------------------------------------
#SEPARATE
void disableautopilot()
{
bAutopilot = 0;
disable_interrupts(INT_TIMER2); // avoid conflict with interrupt routine
slSetRudder = slRudder;
OUTPUT_LOW(pPumpPort); // Clear both Hydraulic Pump bits
OUTPUT_LOW(pPumpStbd); // Don't care which one was set
enable_interrupts(INT_TIMER2);
}
//---------------------------------------------------------------------------
#SEPARATE
void setup()
{
short int btmp;
unsigned int i;
unsigned int Line,Col;
OUTPUT_LOW(pAlarm); // Reset Alarm Output Pin
OUTPUT_LOW(pViper); // Reset Viper Output Pin
OUTPUT_LOW(pPumpPort); // Clear both Hydraulic Pump bits
OUTPUT_LOW(pPumpStbd); // Don't care which one was set
for (i=0; i<32; i++) { // Read from EEPROM
*(((char*)&fVar[0])+i) = READ_EEPROM(i);
}
setfromeeprom();
lcd_init();
SETUP_ADC_PORTS(RA0_RA1_RA3_ANALOG); // 0x84 RA0,RA1 and RA3 Analog Inputs. Ref=Vdd.
/*
SET_TRIS_A(0b11111011); // PORT A Tristate Setup
// 1 RA0 (PIN2) Analog Input AN0
// 1 RA1 (PIN3) Analog Input AN1
// 0 RA2 (PIN4) Output Pump Port
// 1 RA3 (Pin5) Analog Input AN3
// 1 RA4 (Pin6) Input Filter Change Open Drain
// 1 RA5 (Pin7) SS Slave Select
SET_TRIS_C(0b11111111); // PORT C Tristate Setup This is OK
SET_TRIS_C(0b00011111); // PORT C Tristate Setup
// 0 RC0 (Pin11) Output Pump Stbd
// 0 RC1 (Pin12) Output Alarm (CCP2)
// 0 RC2 (Pin13) Output Light (CCP1)
// 1 RC3 (Pin14) SCK/SCL
// 1 RC4 (Pin15)
// 1 RC5 (Pin16)
// 1 RC6 (Pin17) USART TX
// 1 RC7 (Pin18) USART RX
*/
setup_counters(RTCC_INTERNAL, RTCC_DIV_64); // Set up timer 0 as timer ticking every 12.8us
enable_interrupts(INT_RTCC); // enable timer 0 interrupt
ext_int_edge(H_TO_L); // Set PIN B0 interrupt from High to Low
enable_interrupts(INT_EXT); // Enable PIN B0 interrupt
enable_interrupts(int_rda); // Serial Receive data Interrupt
// Timer 2 used for PWM CCP1 freq = 1.25kHz (2.5kHz)
SETUP_TIMER_2(T2_DIV_BY_16, 250, 2); // Interrupt every 4ms. (4)
enable_interrupts(INT_TIMER2); // Enable Timer 2 interrupt
SETUP_CCP1(CCP_PWM);
SET_PWM1_DUTY(cLight); // Duty cycle to 50%
enable_interrupts(GLOBAL); // Enable all enabled interrupts
}
//---------------------------------------------------------------------------
#SEPARATE
void ChangeRudder(signed long slR)
{
signed long slTmp;
disable_interrupts(INT_TIMER2); // avoid conflict with interrupt routine
slTmp = slSetRudder;
enable_interrupts(INT_TIMER2);
slTmp = slTmp + slR;
if (slTmp > MAXRUDDER) slTmp = MAXRUDDER;
if (slTmp < -MAXRUDDER) slTmp = -MAXRUDDER;
disable_interrupts(INT_TIMER2); // avoid conflict with interrupt routine
slSetRudder = slTmp; // New Rudder Setpoint
enable_interrupts(INT_TIMER2);
}
//---------------------------------------------------------------------------
#SEPARATE
void SetRudder(signed long slR)
{
if (slR > MAXRUDDER) slR = MAXRUDDER;
if (slR < -MAXRUDDER) slR = -MAXRUDDER;
disable_interrupts(INT_TIMER2); // avoid conflict with interrupt routine
slSetRudder = slR; // New Rudder Setpoint
enable_interrupts(INT_TIMER2);
}
//---------------------------------------------------------------------------
#SEPARATE
void HandleCmd()
{
unsigned int i;
signed long j;
switch (cMenuMode) {
//-------------------------------------------------------------
case 0: // Main Menu
case 2: // Diag Menu
switch (cCmdBits) {
case 0x15: // On/Off Button
disableAutopilot();
break;
case 0x29: // menu button
if (cMenuMode) { // Must be 2
cMenuMode = 0;
} else { // is 0
cMenuMode = 1;
}
lcd_putc('\f');
break;
case 0x35: // "^" button
if (bAutoPilot) {
slSetPoint += 10;
} else {
ChangeRudder(10);
}
break;
case 0x36: // "v" button
if (bAutoPilot) {
slSetPoint -= 10;
} else {
ChangeRudder(-10);
}
break;
case 0x33: // "->" button
if (bAutoPilot) {
slSetPoint += 50;
slHeadingIntegral = 0;
} else {
ChangeRudder(30);
}
break;
case 0x34: // "<-" button
if (bAutoPilot) {
slSetPoint -= 50;
slHeadingIntegral = 0;
} else {
ChangeRudder(-30);
}
break;
case 0x5A: // "Enter" button
slSetPoint = ((slHeading+5) / 10) * 10;
resetall();
bAutopilot = 1;
break;
case 0x57: // "Input" Increase Viper Period
if (cViperPeriod < 15) {
cViperPeriod++;
} else {
cViperPeriod = 0;
}
cViperCnt = 1; // Make Viper start at once
break;
case 0x60: // "APA" Decrease Viper Period
if (cViperPeriod > 0) {
cViperPeriod--;
} else {
cViperPeriod = 15;
}
cViperCnt = 1; // Make Viper start at once
break;
case 0x3A: // "D. Keystone" Run Viper Once
bViperOnce = 1;
OUTPUT_HIGH(pViper); // Run Viper 1 second
break;
case 0x67: // "Freeze" Enable/Disable Alarm
if (bAlarmEnable) {
bAlarmEnable = 0;
} else {
bAlarmEnable = 1;
}
break;
case 0x6A: // "+" button
cLight = (cLight+1)*2 - 1; // 0 1 3 7 15 31 63 127 255 // (255+1)*2 - 1 = 255 !
SET_PWM1_DUTY(cLight); // Set Duty cycle
break;
case 0x6B: // "-" button
cLight = (cLight)/2; // 255 127 63 31 15 7 3 1 0 // 0/2 = 0
SET_PWM1_DUTY(cLight); // Set Duty cycle
break;
case 0x7B: // Reset button
resetall();
break;
default:
disableAutopilot();
break;
} // switch (cCmdBits)
norm3600(slSetPoint);
break;
//-------------------------------------------------------------
case 1: // Configuration menu to set eeprom values
switch (cCmdBits) {
case 0x29: // menu button
cMenuMode = 2;
lcd_putc('\f');
break;
case 0x67: // "Freeze" button used as "SAVE"
for (i=0; i<32; i++) {
WRITE_EEPROM(i, *(((char*)&fVar[0])+i));
}
break;
case 0x57: // "Input" button used as "Restore from EEPROM"
for (i=0; i<32; i++) {
*(((char*)&fVar[0])+i) = READ_EEPROM(i);
}
break;
case 0x5A: // "Enter" button
fVar[cIdx] = 0.0;
break;
case 0x7B: // "Reset" button sets all to Zero
for (i=0; i<8; i++) {
fVar[i] = 0.0;
}
break;
case 0x6A: // "+" button advance 0 1 2 3 4 5 6 7
if (cIdx<7) cIdx += 1;
break;
case 0x6B: // "-" button decrease 7 6 5 4 3 2 1 0
if (cIdx>0) cIdx -= 1;
break;
case 0x33: // "->" button
fVar[cIdx] = fVar[cIdx] + 0.01;
break;
case 0x34: // "<-" button
fVar[cIdx] = fVar[cIdx] - 0.01;
break;
case 0x35: // "up" button
fVar[cIdx] = fVar[cIdx] + 1.0;
break;
case 0x36: // "down" button
fVar[cIdx] = fVar[cIdx] - 1.0;
break;
default:
disableAutopilot();
break;
} // switch (cCmdBits)
setfromeeprom();
break;
//-------------------------------------------------------------
} // switch (cMenuMode)
}
//---------------------------------------------------------------------------
/*
#SEPARATE
void ()
{
}
*/
//---------------------------------------------------------------------------
#SEPARATE
void PID()
{
signed long slHdgAbove3;
signed long slHdgUnder3;
if (!bAutoPilot) {
slSetPoint = slHeading;
} // if (!bAutoPilot) {
slHeadingError = slHeading - slSetPoint; // P
norm1800(slHeadingError);
slHeadingIntegral = slHeadingIntegral + slHeadingError; // I
slROT = slHeading - slOldHeading; // D
norm1800(slROT);
if (slHeadingError < 0) {
if (slHeadingError < -30) {
slHdgAbove3 = slHeadingError + 30;
slHdgUnder3 = -30;
} else {
slHdgAbove3 = 0;
slHdgUnder3 = slHeadingError;
}
} else {
if (slHeadingError > 30) {
slHdgAbove3 = slHeadingError - 30;
slHdgUnder3 = 30;
} else {
slHdgAbove3 = 0;
slHdgUnder3 = slHeadingError;
}
}
fRudder = -( fVar[0] * slHdgUnder3 * labs(slHdgUnder3) + fVar[4] * slHdgAbove3 ) / fVar[3]; // (P^2 + P)/ Speed
fRudder = fRudder - fVar[2] * slROT; // D
slOldHeading = slHeading;
}
//---------------------------------------------------------------------------
#SEPARATE
void CalcRudder()
{
short int btmp;
unsigned int i;
// The hydraulic system have small leaks in the manual steering pumps which
// give a drifting rudder. If this is not compensated in some way, the rudder angle
// will grow continuously and give misleading values, and finally overflow.
// The following calculation "filters in" a value for the rudder calculated
// from the Rate of Turn. The formula for ROT was found by a turning
// test with the boat. The ROT is fairly proportional with the Rudder Angle within
// at least +/- 20 degrees. The ROT is also approximately proportional with the boat speed
// For my boat the boat speed used with autopilot varies between 6 and 12 knots.
//
// All this to avoid making a sensor for the rudder angle!
// This will of course also defeat the integral part of the PID regulator!
// I'm planning to use the Cross Track Error and True Course output from
// the GPS to correct for that. In the meantime it is nice to have a boat
// which will keep its heading relatively constant, even if you have to correct
// manually for wind and current.
//
// Note that all rudder angles are in 1.7 millisecond movements. The pump will move the
// rudder from fully port to fully starboard in about 3 sec.
// Total rudder angle is +/- 750, which is approximately +/-35 degrees
// For simplicity I divide by 2 (20) for +/- 37.5 as rudder angle for display
//
// Example from ROT test. (steps of 3.3deg rudder equals one spoke on the steering wheel).
// 1500 RPM 5.8kts: 3200 RPM 12kts:
// Rudder ROT Rudder ROT
// 3.3deg. stbd 3deg./s - -
// 3.3deg. port 3deg./s 3.3deg. port 6deg./s
// 6.6deg. port 6deg./s 6.6deg. port 12deg./s
//
// Rudder Angle(Steps) = ROT * (MaxSpeed/Speed) * 0.55 * 20 // for range +/- 750 for rudder angle
disable_interrupts(INT_TIMER2); // avoid conflict with interrupt routine
slRud = slRudder; // get actual rudder angle
enable_interrupts(INT_TIMER2);
slCalcRudder = slRudFromROTCalc * slROT; // use precalculated slRudFromROTCalc which include division by speed!
slCalcRudder = (slCalcRudder + 15*slRud) / 16; // "filter in" the calculated rudder
slRud = fRudder; // slRud is now used to make int of new rudder setting from PID
if (slRud > 300) { // Limit Rudder angle to +/-15 degrees.
slRud = 300;
} else if (slRud < -300) {
slRud = -300;
}
if (bAutoPilot) {
disable_interrupts(INT_TIMER2); // avoid conflict with interrupt routine
slSetRudder = slRud; // set rudder setpoint from PID
slRudder = slCalcRudder; // set the actual rudder angle from calculated
enable_interrupts(INT_TIMER2);
}
}
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
main() {
short int btmp;
unsigned int i;
unsigned int Line,Col;
setup();
while (TRUE) {
//
// Now we must handle compass input (10 times per second)
//
if (bCompassReceived) { // Have we received an NMEA compass message
bCompassReceived = 0; // Clear the bit
bFirstCompass = 1; // We have received one compass telegram
#ifdef FILTER_COMPASS
slNewHeading = atol((char*)&cBuffer[0]);
slDiff = slNewHeading - slHeading; // Handle change through 0,360
if (slDiff > 1800) {
slNewHeading = slNewHeading - 3600;
} else if (slDiff <= -1800) {
slNewHeading = slNewHeading + 3600;
}
slHeading = (slNewHeading + slHeading) / 2; // A little filtering
#else
slHeading = atol((char*)&cBuffer[0]); // No filtering
#endif
norm3600(slHeading);
cCount10 = cCount10 + 1;
if (cCount10 > 9) { // Following code executed every second
cCount10 = 0;
// Handle the Windscreen Viper
if (bViperOnce) { // Viper Once pressed
// OUTPUT_HIGH(pViper); // Run Viper 1 second
bViperOnce = 0; // Reset request
} else { // No, Normal Period Viper
if (cViperPeriod == 0) { // Stop Viper
OUTPUT_LOW(pViper);
// cViperCnt = cViperPeriod; // Reset Timer
} else if (cViperPeriod == 1) { // Run Viper All the time
OUTPUT_HIGH(pViper);
// cViperCnt = cViperPeriod; // Reset Timer
} else { // Period from 2 - 20 sec.
if (--cViperCnt == 0) { // Timer to zero
OUTPUT_HIGH(pViper); // Run Viper 1 second
cViperCnt = cViperPeriod; // Reset Timer
} else {
OUTPUT_LOW(pViper); // Stop Viper
}
}
} // if (bViperOnce) {
if (bAutopilot && bAlarmEnable) { // Is this program controlling the boat ?
if (cCompassReceive < 250) cCompassReceive++; // Increment seconds since last button pressed
if (cCompassReceive > 110) { // More Than 75 seconds since last button press
OUTPUT_HIGH(pAlarm); // Set Alarm Output Pin High for continious alarm
} else if (cCompassReceive > 100) { // More Than 70 seconds since last button press
OUTPUT_HIGH(pAlarm); // Set Alarm Output Pin High
SET_PWM1_DUTY(cLight); // Set Duty cycle back to set value
//delay_ms(1); // Alarm lasts 1 ms, still hardly bearable!
delay_us(250); // Alarm lasts 250us, now quite bearable!
OUTPUT_LOW(pAlarm); // Set Alarm Output Pin Low
} else if (cCompassReceive > 90) { // More Than 60seconds since last button press
cError = 2; // Set Error message to display "Late Button Press!"
if (bToggleLight) {
bToggleLight = 0;
if (clight) { // Is the light on at all
SET_PWM1_DUTY(0); // Set Duty cycle for black display
} else { // No Light on, probably dark outside
SET_PWM1_DUTY(2); // Set Duty cycle for weak light
}
} else {
bToggleLight = 1;
SET_PWM1_DUTY(cLight); // Set Duty cycle for selected light level
}
}
} // (bAutopilot && bAlarmEnable && bFirstCompass) {
PID();
CalcRudder();
} // (cCount10 > 9) {
} else { // if (bCompassReceived) {
// The bFirstCompass is used to avoid the initial compass alarm we
// get because the compass takes about 3 seconds to start
if (bCompassAlarm && bFirstCompass) {
OUTPUT_HIGH(pAlarm); // Set Alarm Output Pin High
bCompassAlarm = 0; // Reset Error Bit
delay_ms(3); // Alarm lasts 5 ms
cError = 1; // Set Error message to display
OUTPUT_LOW(pAlarm); // Set Alarm Output Pin Low
}
} // if (bCompassReceived) {
// Now we must handle command input
if (bCmdReceived) {
bCmdReceived = 0; // Clear indicator bit
OUTPUT_LOW(pAlarm); // Reset Alarm Output Pin
cCompassReceive = 0; // Reset the time since last command
SET_PWM1_DUTY(cLight); // Set Duty cycle back to set value
if (cError == 0) {
HandleCmd();
} else {
cError = 0; // Reset Error Message
}
} // if (bCmdReceived) {
// Now check the filter Alarm input (if not alarm already)
if (!bFilterAlarm) { // Filter alarm not issued yet?
if (!INPUT(pFilter)) { // Check if low
bFilterAlarm = 1; // Indicate filter alarm issued
OUTPUT_HIGH(pAlarm); // Set Alarm Output Pin High
delay_ms(25); // Alarm lasts 25 ms
cError = 3; // Set Error message to display "Filter Change!"
OUTPUT_LOW(pAlarm); // Set Alarm Output Pin Low
}
}
switch (cMenuMode) {
//----------------------------------------------------------------------------
case 0: // Main Menu
// Handle Line 1
lcd_gotoxy(1,1);
printf(lcd_putc,"HDG");
#ifdef DPRINT6
print6(slHeading);
#else
printf("%6ld",slHeading);
#endif
lcd_gotoxy(12,1);
printf(lcd_putc,"ROT");
#ifdef DPRINT6
print6(slROT);
#else
printf("%6ld",slROT);
#endif
// Handle Line 2
lcd_gotoxy(1,2);
printf(lcd_putc,"SET");
if (bAutopilot) {
printf(lcd_putc,"%4ld", (slSetPoint/10));
} else {
printf(lcd_putc," ---");
}
lcd_gotoxy(12,2);
printf(lcd_putc,"INT");
#ifdef DPRINT6
print6(slHeadingIntegral);
#else
printf("%6ld",slHeadingIntegral);
#endif
/*
lcd_gotoxy(12,2);
printf(lcd_putc,"RRT");
#ifdef DPRINT6
print6(slRRT);
#else
printf("%6ld",slRRT);
#endif
*/
// Handle Line 3
switch (cError) {
case (0):
lcd_gotoxy(1,3);
printf(lcd_putc,"ERR");
#ifdef DPRINT6
print6(slHeadingError);
// printf(" ");
#else
printf("%6ld",slHeadingError);
#endif
lcd_gotoxy(10,3);
printf(lcd_putc," Cal");
#ifdef DPRINT6
print6(slCalcRudder/2);
#else
printf("%6ld",slCalcRudder/2);
#endif
break;
case (1): // Compass not received
lcd_gotoxy(1,3);
printf(lcd_putc,"Late Compass! ");
break;
case (2): // Late Button Press
lcd_gotoxy(1,3);
printf(lcd_putc,"Late Button Press! ");
break;
case (3): // Filter Change
lcd_gotoxy(1,3);
printf(lcd_putc,"Filter Change! ");
break;
}
// Handle Line 4
lcd_gotoxy(1,4);
printf(lcd_putc,"RUD");
#ifdef DPRINT6
print6(slSetRudder/2);
print6(slRudder/2);
#else
printf("%6ld",slSetRudder/2);
printf("%6ld",slRudder/2);
#endif
lcd_gotoxy(17,4);
if (cViperPeriod != 0) {
printf(lcd_putc,"%3U", cViperPeriod); // Show Viper Period
} else {
printf(lcd_putc,"%3U", cCompassReceive); // time since button press
}
if (bAlarmEnable) {
lcd_putc('*');
} else {
lcd_putc(' ');
}
break;
//----------------------------------------------------------------------------
case 1: // Config menu
for (i=0; i<8; i++) {
Line = (i & 0x03) + 1;
if (i < 4) {
Col = 1;
} else {
Col = 11;
}
lcd_gotoxy(Col,Line);
lcd_putc(c1[i]);
lcd_putc(c2[i]);
if (i==cIdx) {
printf(lcd_putc,"*%5.2f*", fVar[i]);
} else {
printf(lcd_putc," %5.2f ", fVar[i]);
}
} // of for (i=0....
break;
//----------------------------------------------------------------------------
case 2: // Diagnostic menu
#ifdef DIAG
lcd_gotoxy(1,1);
// printf(lcd_putc,"%2X %3u %3u %3u %3u", k, c1_1, c1_65, c2_2, cX_X);
// printf(lcd_putc,"%2X %3u %3u %3u %3u", k, cPulseState, cSameCnt, cIdleCnt, cX_X);
// printf(lcd_putc,"%2X %3u %3u %3u %3u", cBuffer[0], cNextIn, cSameCnt, cIdleCnt, cX_X);
printf(lcd_putc,"%2X %3u %3u %3u %3u", cBuffer[0], cNextIn, cSameCnt, cIdleCnt, cX_X);
lcd_gotoxy(1,3);
printf(lcd_putc,"SBT=%1u,%2u,%1u,%2X%2X%2X",
cLast2Status, cLast2BitCnt, cLast2StartType, cLast2Bits[2], cLast2Bits[1], cLast2Bits[0]);
#endif
lcd_gotoxy(1,1);
printf(lcd_putc,"HDG");
#ifdef DPRINT6
print6(slHeading);
#else
printf("%6ld",slHeading);
#endif
lcd_gotoxy(11,1);
printf(lcd_putc,"ROT");
print6(slROT);
lcd_gotoxy(1,2);
// printf(lcd_putc,"SET");
// print6(slSetPoint);
printf(lcd_putc,"SET%4ld", (slSetPoint/10));
lcd_gotoxy(11,2);
printf(lcd_putc,"RUD");
print6((slRudder/2));
lcd_gotoxy(11,3);
printf(lcd_putc,"Cal");
print6((slCalcRudder/2));
lcd_gotoxy(1,3);
i = 0;
while (cBuffer[i] != 0) {
printf(lcd_putc,"%C", cBuffer[i]);
i++;
}
printf(lcd_putc, ">");
lcd_gotoxy(1,4);
printf(lcd_putc,"FromROT %4ld ", slRudFromROTCalc);
// printf(lcd_putc,"SQR + Linear P/Speed");
// printf(lcd_putc,"%3D %3D",scSetDelayPort,scSetDelayStbd);
// printf(lcd_putc,"SBT=%1u,%2u,%1u,%2X%2X%2X",
// cLast1Status, cLast1BitCnt, cLast1StartType, cLast1Bits[2], cLast1Bits[1], cLast1Bits[0]);
break;
} // switch (cMenuMode)
} // while (TRUE)
}
|
|
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
