CCS C Software and Maintenance Offers
FAQFAQ   FAQForum Help   FAQOfficial CCS Support   SearchSearch  RegisterRegister 

ProfileProfile   Log in to check your private messagesLog in to check your private messages   Log inLog in 

CCS does not monitor this forum on a regular basis.

Please do not post bug Reports on this forum. Send them to support@ccsinfo.com

Boat Autopilot on 16F876

 
Post new topic   Reply to topic    CCS Forum Index -> Code Library
View previous topic :: View next topic  
Author Message
skarven



Joined: 12 Mar 2009
Posts: 2

View user's profile Send private message

Boat Autopilot on 16F876
PostPosted: Fri Mar 13, 2009 12:20 pm     Reply with quote

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
kai@smsc.no
kaid@broadpark.no



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 &deg)
{
   if (deg >= 3600) deg = deg - 3600;
   if (deg <     0) deg = deg + 3600;
}
//---------------------------------------------------------------------------
#SEPARATE
void norm1800(signed long &deg)
{
   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)
}
Display posts from previous:   
Post new topic   Reply to topic    CCS Forum Index -> Code Library All times are GMT - 6 Hours
Page 1 of 1

 
Jump to:  
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot vote in polls in this forum


Powered by phpBB © 2001, 2005 phpBB Group