/* Title: Docking Station C source for FAU OE AUV Dept. File: DockingStation.C Date: 12/3/04 Rev. A */ #include /* for the special function register declarations */ #include /* for Serial port UART*/ #include /* for the RB0/INT0 interrupt */ #include /* for delays*/ #include /* for serial port command etc*/ #include /* for Health and Powerout monitoring */ #include /* for RTC and DAC */ #include "e:\data\docking\FAU\firmware\dock\DockingStation.h" /* Docking Payload include file*/ #include /* for Data conversions*/ /****************** Top function protypes ***********************/ void IntHighNotUsed (void); /* prototype needed for high interrupt 'goto'*/ void Initialize (void); /* prototype for Initialize function */ void ReadAllInputs (void); /* prototype of function for reading all inputs */ void CommunicationMan (void); /* prototype of function for Communication Manager */ void DetermineMode (void); /* prototype of function for Determining the Mode */ void DetermineOutput (void); /* prototype of function for Determining outputs */ void WriteAllOutput (void); /* prototype of function to write to all outputs */ /****************** Function protypes ***********************/ void ReadStrTimeOut (char); /*Read a string form the USART, return nul if time out*/ /*Pass a w to wait for a string to start*/ /*and any other char to exit if USART not ready*/ /*********************** Input Data *****************************/ char InCommand[3]; /* Command from Serial Port */ char InData[22]; /* command Data */ char InCurrent0[5]; /* Data from SV command*/ char InCurrent1[5]; /* Data from SV command*/ char InProbe[5]; /* Data from SV command*/ char InBrake[5]; /* Data from SV command*/ char InString[22]; /*Data string from USART */ unsigned int V_UnReg; /*Unregulated Voltage 0-102.4V FS, 0.1V per count*/ unsigned int I_Load_40mA; /*Load Current 0 to 40.96A FS, 40mA per count*/ unsigned int I_Probe_4mA; /*Probe Current 0 to 4.096A FS, 4mA per count*/ unsigned int I_Brake_2ma; /*Brake Current 0 to 2.048A FS, 2mA per count*/ unsigned int I_USBL_4ma; /*USBL Current 0 to 4.096A FS, 4mA per count */ unsigned int Resolver0; /*Resolver 0 input*/ unsigned int Resolver1; /*Resolver 1 input*/ char Leak; /*Leak status*/ int Amb_Temp; /*Temperture 0.5156 Deg. C per count and 0 Deg.C = = 131.5 Counts */ char One_Sec_Tick; /*one Second tick from RTC*/ char ProbeState; /* Up = 1, Down = 2, Brake = 0 */ char ProbeStatus; /*********************** Control Data *****************************/ char Mode; /* 0 = Docked-nocharge Mode */ /* 1 = Docked-charge Mode */ /* 2 = Idle Mode */ /* 3 = AUV returing Mode */ /* 4 = Failed-docked Mode */ /* 4 = Failed-nondocked Mode */ char OldMode; /* Old Value */ char NewCommand; /* Flag for new command = 1*/ char NewMode; /* Flag for new mode = 1*/ char ChargingError; /* Flag for Charge error, 1 = error */ char NewSetPoint; /* Current, Probe and Brake set point output*/ char ErrorFlag; char X; unsigned long int Y; char ProbeTimer; char Temp5; /********************* Current Limit Set Points DAC Setpoint Variables **********************/ union HiLoByteInt /* For Int to High and Low byte to DAC*/ { int Setpoint; char HiLoByte[2]; }; union HiLoByteInt SetPtC1; union HiLoByteInt SetPtC2; union HiLoByteInt ProbeSetPt; union HiLoByteInt BrakeSetPt; #define SetPtCurrent0 SetPtC1.Setpoint /*DAC Set point Voltage 12.207mA per count*/ #define LoSetPtCurrent0 SetPtC1.HiLoByte[0] /* 0 to 3.3V = 0 to 50A*/ #define HiSetPtCurrent0 SetPtC1.HiLoByte[1] #define SetPtCurrent1 SetPtC2.Setpoint /* DAC Setpoint Current 12.207mA per count*/ #define LoSetPtCurrent1 SetPtC2.HiLoByte[0] /* 0 to 3.3V = 0 to 50A*/ #define HiSetPtCurrent1 SetPtC2.HiLoByte[1] #define SetPtProbe ProbeSetPt.Setpoint /* DAC Setpoint Probe control*/ #define LoSetPtProbe ProbeSetPt.HiLoByte[0] /* 0 to 3.3V = -5V to +5V */ #define HiSetPtProbe ProbeSetPt.HiLoByte[1] #define SetPtBrake BrakeSetPt.Setpoint /* DAC Setpoint Brake control*/ #define LoSetPtBrake BrakeSetPt.HiLoByte[0] /* 0 to 3.3V = -5V to +5V */ #define HiSetPtBrake BrakeSetPt.HiLoByte[1] /******************* Temp Variables *********************************/ unsigned char TempChar ; /*Char not used*/ /********************************** Interrupt Vectors ******************************/ /* High interrupts Vector at address 0x8. Not used at this time*/ #pragma code HIGH_INTERRUPT_VECTOR = 0x8 void high_ISR (void) { _asm goto IntHighNotUsed _endasm } /************************************************************************************/ /****************************** Start of Program Code *******************************/ /************************************************************************************/ #pragma code /* allow the linker to locate the remaining code */ /* If the buzzer is on, turn it off. If it is off, turn it on.*/ #pragma interrupt IntHighNotUsed void IntHighNotUsed (void) { /* add code here*/ INTCONbits.INT0IF = 0; /* clear flag to avoid another interrupt */ } /*** Use only if High interrupts are needed***/ void EnableHighInterrupts (void) { RCONbits.IPEN = 1; /* enable interrupt priority levels */ INTCONbits.GIEH = 1; /* enable all high priority interrupts */ } /************************************************************************************/ /************************************ Start of Main code ****************************/ /************************************************************************************/ void main (void) { Initialize( ); while (1) { ReadAllInputs(); CommunicationMan(); DetermineMode(); WriteAllOutput(); } } /************************************************************************************/ /****************************** Initialize Micro Routine *****************************/ /************************************************************************************/ void Initialize (void) { /********************** Setup Ports and Peripherals ******************/ /**** Port A Setup ****/ TRISAbits.TRISA4 = 0; /*Make RA4 an output for DAC1 SPI CS */ DAC1_CS_L = 1; /* Deselect DAC 1*/ /**** Port B Setup ****/ ClosePORTB(); /*Disable all Interrupts and Pullups*/ EnablePullups(); /* Enable PullUps on Port B inputs */ TRISBbits.TRISB0 = 1; /*Make RB0 an input pin for RTC_INT test*/ TRISBbits.TRISB1 = 1; /*Make RB1 an input pin for RS232_INT test*/ TRISBbits.TRISB2 = 0; /*Make RB2 an output pin for RS232_CTR0 */ RS232_CTR0 = 1; /* Set control nib to Normal ops */ TRISBbits.TRISB4 = 0; /*Make RB4 an output pin for DAC0_CS_L */ DAC0_CS_L = 1; /* deselect DAC 0 */ TRISBbits.TRISB5 = 0; /*Make RB5 an output pin for RS232_CTR1 */ RS232_CTR1 = 0; /* Set control nib to AUV Mission */ /**** Port C Setup ****/ TRISCbits.TRISC0 = 0; /*Make RC0 an output pin for SPI_RTC_CS */ RTC_CS = 0; /* deselect RTC */ TRISCbits.TRISC1 = 0; /*Make RC1 an output pin for RS232CTR2 */ RS232CTR2 = 0; /* Set control nib to AUV Mission */ TRISCbits.TRISC2 = 0; /*Make RC1 an output pin for RS232CTR3 */ RS232CTR3 = 0; /* Set control nib to AUV Mission */ /**** Port D Setup ****/ TRISDbits.TRISD0 = 0; /*Make RD0 an output pin for PC104_ON */ PC104_ON = on; /* Turn on power to PC104 */ TRISDbits.TRISD1 = 0; /*Make RD1 an output pin for RF_ETHERNET_ON */ PROBE_ON = off; /* Turn off power to PROBE */ TRISDbits.TRISD2 = 0; /*Make RD2 an output pin for MOD_USBL_ON */ MOD_USBL_ON = on; /* Turn on power to ACOUSTIC MODEM and USBL */ TRISDbits.TRISD3 = 1; /*Make RD3 an input pin for Leak error */ TRISDbits.TRISD4 = 0; /* Break Fwd/Rev control */ BRK_FWR_REV_L = 1; /* Set to Fwd*/ TRISDbits.TRISD5 = 0; /* Break power control */ BRK_ON = 1; /* Set to Break power on*/ TRISDbits.TRISD6 = 0; /* Break CURRENTMETER power control */ CURRENTMETER_ON = 0; /* Set to off*/ TRISDbits.TRISD7 = 0; /*Make RD7 an output pin for PWROUT_L */ PWROUT_ON = 0; /* Disable power Output */ /**** Setup USART, 9600 Baud ****/ OpenUSART( USART_TX_INT_OFF & USART_RX_INT_OFF & USART_ASYNCH_MODE & USART_EIGHT_BIT & USART_CONT_RX & USART_BRGH_HIGH, 25 ); /***** Setup Timer 0 *****/ OpenTimer0( TIMER_INT_OFF & T0_16BIT & T0_SOURCE_INT & T0_PS_1_256 ); /* 4MhZ xTAL = 1uSec * 256 = 256uSec per count = 16.777 Sec FS*/ /****** Setup ADC ******/ OpenADC( ADC_FOSC_8 & /* FOSC/8 FOR 4 MHz Xtal/8 = .5MHz = 2uS (ADC min. is 1.6uS)*/ ADC_RIGHT_JUST & ADC_8ANA_0REF, ADC_CH0 & /*Main Power input Voltage 0-102.4V FS, 0.1V per count*/ ADC_CH1 & /*Power out Current 0 to 40.96A FS, 40mA per count*/ ADC_CH2 & /*Probe Current 0 to 4.096A FS, 4mA per count*/ ADC_CH3 & /*Resolver 0 input*/ ADC_CH4 & /*Resolver 1 input*/ ADC_CH5 & /*Temperture 0.5156 Deg. C per count and 0 Deg.C = = 131.5 Counts */ ADC_CH6 & /*Brake Current 0 to 2.048A FS, 2mA per count*/ ADC_CH7 & /*USBL Current 0 to 4.096A FS, 4mA per count */ ADC_INT_OFF); /****** Setup SPI ******/ OpenSPI( SPI_FOSC_16, /* 4MHz/16 = 250KHz SPI Clock Rate */ MODE_00, /*SSPSTAT register = 0x40 Data transmitted on rising edge of clock SSPCON1 register = 0x00 Idle state of clock is low*/ SMPMID); /* Input data sampled a middle of data output*/ /*********************** Initialize Real Time Clock (RTC) ***********************/ /* Now set the RTC to alarm every second */ RTC_CS = 1; /* Select RTC */ WriteSPI(0x8B); /* Set RTC address to start at Sec Alarm 1 Register address */ WriteSPI(0x80); /* Write RTC Seconds Alarm register mask bit */ WriteSPI(0x80); /* Write RTC minutes Alarm register mask bit */ WriteSPI(0x80); /* Write RTC hour Alarm register mask bit */ WriteSPI(0x80); /* Write RTC days Alarm register mask bit */ WriteSPI(0x06); /* Write RTC Control Reg data till no collision */ RTC_CS = 0; /* DeSelect RTC */ /******************************* Initialize DAC0 and DAC 1 **********************************/ /*DAC0 channel A is the Power Output #0 Current Setpoint 0 to 3.3V = 0 to 50A*/ /*DAC0 channel B is the Power Output #1 Current setpoint 0 to 3.3V = 0 to 50A*/ /* Set both to zero at reset*/ /*Note the 4 MSBs of the 16 bite value are the address pointer in the DAC*/ /* DAC A Voltage setpoint = 0000 DDDD DDDD DDDD */ /* DAC B Current setpoint = 0001 DDDD DDDD DDDD */ /* DAC O/P Wakeup = 1111 XXXX XXX1 X000 */ DAC0_CS_L = 0; /* select DAC */ WriteSPI(0x00); /*Set DAC A to 0 */ WriteSPI(0x00); /*Set DAC A to 0 */ DAC0_CS_L = 1; /* Deselect DAC */ DAC0_CS_L = 0; /* select DAC */ WriteSPI(0x10); /*Set DAC B to 0*/ WriteSPI(0x00); /*Set DAC B to 0*/ DAC0_CS_L = 1; /* Deselect DAC */ DAC0_CS_L = 0; /* select DAC */ WriteSPI(0xF0); /*Wake DAC A & B*/ WriteSPI(0x10); /*Wake DAC A & B*/ DAC0_CS_L = 1; /* Deselect DAC */ /*DAC1 channel A is the DAC Setpoint Probe control 0 to 3.3V = -5V to +5V*/ /*DAC1 channel B is the DAC Setpoint Brake control 0 to 3.3V = -5V to +5V*/ /* Set both to 1/2 scale for zero output*/ DAC1_CS_L = 0; /* select DAC */ WriteSPI(0x08); /*Set DAC A to 1/2 scale */ WriteSPI(0x00); /*Set DAC A to 1/2 scale*/ DAC1_CS_L = 1; /* Deselect DAC */ DAC1_CS_L = 0; /* select DAC */ WriteSPI(0x18); /*Set DAC B to 1/2 scale*/ WriteSPI(0x00); /*Set DAC B to 1/2 scale*/ DAC1_CS_L = 1; /* Deselect DAC */ DAC1_CS_L = 0; /* select DAC */ WriteSPI(0xF0); /*Wake DAC A & B*/ WriteSPI(0x10); /*Wake DAC A & B*/ DAC1_CS_L = 1; /* Deselect DAC */ /*********************************** Initialize Variables *************************/ strcpypgm2ram(InCommand,""); /* Set Input Command to No new command*/ strcpypgm2ram(InString,""); /* Set InString to null */ strcpypgm2ram(InCurrent0,""); /* Set point current1 */ strcpypgm2ram(InCurrent1,""); /* Set point current0 */ strcpypgm2ram(InProbe,""); /* Set point Probe */ strcpypgm2ram(InBrake,""); /* Set point Brake */ Mode = 0; V_UnReg = 0; /*Main Power input Voltage 0-102.4V FS, 0.1V per count*/ I_Load_40mA = 0; /*Power out Current 0 to 40.96A FS, 40mA per count*/ I_Probe_4mA = 0; /*Probe Current 0 to 4.096A FS, 4mA per count*/ Resolver0 = 0; /*Resolver 0 input*/ Resolver1 = 0; /*Resolver 1 input*/ I_Brake_2ma = 0; /*Brake Current 0 to 2.048A FS, 2mA per count*/ I_USBL_4ma = 0; /*USBL Current 0 to 4.096A FS, 4mA per count */ Amb_Temp = 0; /*Temperture 0.5156 Deg. C per count and 0 Deg.C = = 131.5 Counts */ Leak = 0; One_Sec_Tick = 0; ProbeState = 0; ProbeTimer = 0; ProbeStatus = 0; /* 0 = unknow 1 = up and 2 = down*/ } /*^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^*/ /*^^^^^^^^^^^^^^^^^^^^^^ End of Initialize Micro Routine ^^^^^^^^^^^^^^^^^^^^^^^^^^*/ /*^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^*/ /************************************************************************************/ /****************************** Read all Inputs Routine *****************************/ /************************************************************************************/ void ReadAllInputs (void) { NewCommand = no; /*************** Read RTC and toggle 1 second tick **************/ if (RTC_INT == 0) { /* Read RTC alarm register to clear RTC_INT and Set 1 sec tick*/ One_Sec_Tick = 1; /*Set 1 second tick*/ /* Clear RTC_INT by reading the second alarm reg*/ RTC_CS = 1; /* Select RTC */ WriteSPI(0x0B); /* Set RTC address to start at Sec Alarm 1 Register address */ TempChar = ReadSPI(); /* Read RTC Seconds Alarm register mask bit */ RTC_CS = 0; /* DeSelect RTC */ } else /* clear 1 second tick */ { One_Sec_Tick = 0; } /******************* Read Health and Powerout*******************/ if (One_Sec_Tick) { SetChanADC(ADC_CH0); Delay10TCYx(1); /*Delay for 10 instr. cycles = 10uS*/ ConvertADC(); while (BusyADC()); V_UnReg = ReadADC(); /*PwrOut Voltage 0-102.4V FS, 0.1V per count*/ SetChanADC(ADC_CH1); Delay10TCYx(1); /*Delay for 10 instr. cycles = 10uS*/ ConvertADC(); while (BusyADC()); I_Load_40mA = ReadADC(); /*Power out Current 0 to 40.96A FS, 40mA per count*/ SetChanADC(ADC_CH2); Delay10TCYx(1); /*Delay for 10 instr. cycles = 10uS*/ ConvertADC(); while (BusyADC()); I_Probe_4mA = ReadADC(); /*Probe Current 0 to 4.096A FS, 4mA per count*/ SetChanADC(ADC_CH3); Delay10TCYx(1); /*Delay for 10 instr. cycles = 10uS*/ ConvertADC(); while (BusyADC()); Resolver0= ReadADC(); /*Resolver 0 input*/ SetChanADC(ADC_CH4); Delay10TCYx(1); /*Delay for 10 instr. cycles = 10uS*/ ConvertADC(); while (BusyADC()); Resolver1 = ReadADC(); /*Resolver 1 input*/ SetChanADC(ADC_CH5); Delay10TCYx(1); /*Delay for 10 instr. cycles = 10uS*/ ConvertADC(); while (BusyADC()); Amb_Temp = ReadADC();/*Temperture 0.5156 Deg. C per count and 0 Deg.C = = 131.5 Counts */ SetChanADC(ADC_CH6); Delay10TCYx(1); /*Delay for 10 instr. cycles = 10uS*/ ConvertADC(); while (BusyADC()); I_Brake_2ma = ReadADC(); /*Brake Current 0 to 2.048A FS, 2mA per count*/ SetChanADC(ADC_CH7); Delay10TCYx(1); /*Delay for 10 instr. cycles = 10uS*/ ConvertADC(); while (BusyADC()); I_USBL_4ma = ReadADC(); /*USBL Current 0 to 4.096A FS, 4mA per count*/ } if (ProbeTimer != 0) /* Read allways if Probe motor runing */ { SetChanADC(ADC_CH2); Delay10TCYx(1); /*Delay for 10 instr. cycles = 10uS*/ ConvertADC(); while (BusyADC()); I_Probe_4mA = ReadADC(); /*Probe Current 0 to 4.096A FS, 4mA per count*/ } } /************************************************************************************/ /****************************** Get New Command *************************************/ /************************************************************************************/ void CommunicationMan (void) { if (!NewCommand) { /****************** Get Command from serial port ***************/ ReadStrTimeOut('T'); /*check for command string*/ if (strlen(InString) == 2) { strcpy(InCommand,InString); if(!strcmppgm2ram(InCommand,"ss")) { putrsUSART("SS OK\r\n"); putrsUSART("enter mode\r\n"); putrsUSART("docked-nocharge, docked-charge, idle, auv-returning, failed-docked, failed-nondocked\r\n"); ReadStrTimeOut('W'); /*Get New mode type string*/ if( (!strcmppgm2ram(InString,"docked-nocharge")) || (!strcmppgm2ram(InString,"docked-charge")) || (!strcmppgm2ram(InString,"idle"))|| (!strcmppgm2ram(InString,"auv-returning"))|| (!strcmppgm2ram(InString,"failed-docked"))|| (!strcmppgm2ram(InString,"failed-nondocked")) ) { strcpy(InData,InString); putrsUSART("GOOD\r\n"); NewCommand = yes; } } if(!strcmppgm2ram(InCommand,"pc")) { putrsUSART("SP OK\r\n"); putrsUSART("ii.ii ii.ii\r\n"); ReadStrTimeOut('W'); /*Get Current set points*/ if( strcmppgm2ram(InString,"e1") && (strlen(InString) == 11) ) { InCurrent0[0] = InString[0]; InCurrent0[1] = InString[1]; InCurrent0[2] = InString[3]; InCurrent0[3] = InString[4]; InCurrent0[4] = '\0'; InCurrent1[0] = InString[6]; InCurrent1[1] = InString[7]; InCurrent1[2] = InString[9]; InCurrent1[3] = InString[10]; InCurrent1[4] = '\0'; SetPtCurrent0 = (atoi( InCurrent0)*4)/5; SetPtCurrent1 = (atoi( InCurrent1)*4)/5; if ((SetPtCurrent0 >= 0) && (SetPtCurrent0 < 4096) && (SetPtCurrent1 >= 0) && (SetPtCurrent1 <4096) ) { NewCommand = yes; putrsUSART("GOOD\r\n"); NewSetPoint = 1; } } } /* Used to read probe status */ if(!strcmppgm2ram(InCommand,"qp")) { if ((ProbeStatus == 0) && (ProbeState != 1) && (ProbeState != 2)) { putrsUSART("unknown\r\n"); } if ((ProbeStatus == 1) && (ProbeState != 1) && (ProbeState != 2)) { putrsUSART("up\r\n"); } if ((ProbeStatus == 2) && (ProbeState != 1) && (ProbeState != 2)) { putrsUSART("down\r\n"); } if (ProbeState == 1) { putrsUSART("raising\r\n"); } if (ProbeState == 2) { putrsUSART("lowering\r\n"); } NewCommand = yes; } /* note set point +/- 5.00 V */ if ( (!strcmppgm2ram(InCommand,"sp")) && (Mode == 0) ) { putrsUSART("SP OK\r\n"); putrsUSART("up/down/brake\r\n"); ReadStrTimeOut('W'); /*Get New probe state string*/ if(!strcmppgm2ram(InString,"up")) { ProbeState = 1; putrsUSART("GOOD\r\n"); ProbeTimer = 15 ; /*set time out to 15 sec */ SetPtProbe = ((512 + 408)*4) ; /* + 3.75V up */ NewSetPoint = 1; NewMode = 1; NewCommand = yes; } if(!strcmppgm2ram(InString,"down")) { ProbeState = 2; putrsUSART("GOOD\r\n"); ProbeTimer = 15 ; /*set time out to 15 sec */ SetPtProbe = ((512 - 375 )*4) ; /* -3.75V down */ NewSetPoint = 1; NewMode = 1; NewCommand = yes; } if(!strcmppgm2ram(InString,"brake")) { ProbeState = 0; putrsUSART("GOOD\r\n"); SetPtProbe = (512 * 4) ; /* + 0V brake */ NewSetPoint = 1; NewMode = 1; NewCommand = yes; } } if(!strcmppgm2ram(InCommand,"qr")) { char LoopCount; Resolver0 = 0; Resolver1 = 0; for (LoopCount = 1; LoopCount <= 32; ++LoopCount) { SetChanADC(ADC_CH3); Delay10TCYx(1); /*Delay for 10 instr. cycles = 10uS*/ ConvertADC(); while (BusyADC()); Resolver0= Resolver0 + ReadADC(); /*Resolver 0 input*/ SetChanADC(ADC_CH4); Delay10TCYx(1); /*Delay for 10 instr. cycles = 10uS*/ ConvertADC(); while (BusyADC()); Resolver1 = Resolver1 + ReadADC(); /*Resolver 1 input*/ } Resolver0 = Resolver0 / 32 ; Resolver1 = Resolver1 / 32 ; /* Resolver is now 0 to 1023 */ if ((Resolver0 < 1020)&& (Resolver0 < 1020)) { if ((Resolver0 > 40) || (Resolver1 > 40)) { if ( Resolver0 < Resolver1) /* AUV is not passed dock center position */ { if ((Resolver0/8) >= (Resolver1 - Resolver0)) /* near if within 12.5%*/ { putrsUSART("good \r\n"); } else { putrsUSART("near \r\n"); } } else /* AUV is passed dock center position */ { putrsUSART("good \r\n"); } } else { putrsUSART("far \r\n"); } NewCommand = yes; } } if(!strcmppgm2ram(InCommand,"rs")) { if (Mode == 0) { putrsUSART("The state is docked_nocharge\r\n"); } if (Mode == 1) { putrsUSART("The state is docked_charge\r\n"); } if (Mode == 2) { putrsUSART("The state is idle\r\n"); } if (Mode == 3) { putrsUSART("The state is auv_returning\r\n"); } if (Mode == 4) { putrsUSART("The state is failed_docked\r\n"); } if (Mode == 5) { putrsUSART("The state is failed_nondocked\r\n"); } putrsUSART("Leak Status is "); if (Leak) { putrsUSART("Bad"); } else { putrsUSART("Good"); } putrsUSART("\r\n"); putrsUSART("Temperature is "); Y = (Amb_Temp - 131)/2; ultoa(Y, InData); putsUSART(InData); putrsUSART(" C\r\n"); putrsUSART("The Shore power voltage is "); Y = V_UnReg/10; ultoa(Y, InData); putsUSART(InData); putrsUSART("."); Y = V_UnReg - (Y * 10); ultoa(Y, InData); putsUSART(InData); putrsUSART(" V\r\n"); putrsUSART("Power Out Current is (0 to 40.95A) "); Y = (I_Load_40mA * 4)/100; ultoa(Y, InData); putsUSART(InData); putrsUSART("."); Y = (I_Load_40mA * 4) - (Y * 100); ultoa(Y, InData); putsUSART(InData); putrsUSART(" A\r\n"); putrsUSART("USBL/Modem Current is (0 to 4.095A) "); Y = (I_USBL_4ma * 4)/1000; ultoa(Y, InData); putsUSART(InData); putrsUSART("."); Y = (I_USBL_4ma * 4) - (Y * 1000); ultoa(Y, InData); putsUSART(InData); putrsUSART(" A\r\n"); putrsUSART("Probe motor Current is (0 to 4.095A) "); Y = (I_Probe_4mA * 4)/1000; ultoa(Y, InData); putsUSART(InData); putrsUSART("."); Y = (I_Probe_4mA * 4) - (Y * 1000); ultoa(Y, InData); putsUSART(InData); putrsUSART(" A\r\n"); putrsUSART("Brake motor Current is (0 to 2.047A) "); Y = (I_Brake_2ma * 2)/1000; ultoa(Y, InData); putsUSART(InData); putrsUSART("."); Y = (I_Brake_2ma * 2) - (Y * 1000); ultoa(Y, InData); putsUSART(InData); putrsUSART(" A\r\n"); putrsUSART("Resolver #0 count is (0 to 1023) "); ultoa(Resolver0, InData); putsUSART(InData); putrsUSART(" \r\n"); putrsUSART("Resolver #1 count is (0 to 1023) "); ultoa(Resolver1, InData); putsUSART(InData); putrsUSART(" \r\n"); NewCommand = yes; } if(!strcmppgm2ram(InCommand,"dc")) { putrsUSART("Command Set:\r\n"); putrsUSART("ss = Set State\r\n"); putrsUSART("sc = Set Clock\r\n"); putrsUSART("rc = Read Clock\r\n"); putrsUSART("pc = Set Power Out Currents\r\n"); putrsUSART("rs = Read Status\r\n"); putrsUSART("sp = Set probe up/Down/brake\r\n"); putrsUSART("qr = Query Resolver, good/near/far\r\n"); putrsUSART("qp = Query Probe Status\r\n"); NewCommand = yes; } if (NewCommand == no) { putrsUSART("Error\r\n"); strcpypgm2ram(InCommand,""); } } else { strcpypgm2ram(InCommand,""); if (strlen(InString) != 0) { putrsUSART("Error\r\n"); } } } } /************************************************************************************/ /******************** Determine Mode and output Values *****************************/ /************************************************************************************/ void DetermineMode (void) { /********** Probe Timer and controller *******************/ if ((One_Sec_Tick == 1) && (ProbeTimer != 0)) { --ProbeTimer; } if(((ProbeTimer == 0) ||( I_Probe_4mA > 200 )) && ProbeState != 0 ) /* stop motor, current limit @ ~800mA (was ~408mA) */ { /* problem is ~600mAAC + 300mADC current @ 29kHz */ Temp5 = ProbeTimer; if (ProbeState == 1) /* State was moving up */ { if (ProbeTimer < 13 ) { ProbeStatus = 1; /* Status = Up*/ } else { ProbeStatus = 0; /* unknow*/ } } if (ProbeState == 2) /* State was moving up *//*probe state was up */ { if (ProbeTimer < 13 ) { ProbeStatus = 2; /* Status = Up*/ } else { ProbeStatus = 0; /* unknow*/ } } ProbeState = 0; SetPtProbe = (512 * 4) ; /* + 0V brake */ NewSetPoint = 1; NewMode = 1; NewCommand = yes; } /********************* Determine Mode ************************/ OldMode = Mode; /* If new command update mode. If health errors reset mode to safe mode.*/ if (NewCommand == 1) { if (!strcmppgm2ram(InString,"docked-nocharge")) {Mode = 0;} if (!strcmppgm2ram(InString,"docked-charge")) {Mode = 1;} if (!strcmppgm2ram(InString,"idle")) {Mode = 2;} if (!strcmppgm2ram(InString,"auv-returning")) {Mode = 3;} if (!strcmppgm2ram(InString,"failed-docked")) {Mode = 4;} if (!strcmppgm2ram(InString,"failed-nondocked")){Mode = 5;} } if (Mode == 4) { /* <<<<<< Temp. del. for testing >>>>>>>>*/ /* if (V_UnReg < 654) /* Voltage input to 1KW charger*/ /* { /*Unregulated Vin 0-102.4V = FS, = 0 to 1024 counts*/ /* Mode = 3; /* Less then 65V*/ /* } */ if (I_Load_40mA > 1000) /*Load Current 0 to 40.96A FS, 40mA per count, 30A/.03 = 1000 counts */ { Mode = 4; /* > 30Amps */ } } if (Leak) /* Leak*/ { Mode = 4; /* Safe mode all power off*/ } if (Amb_Temp > 238) /*Temperture 0.5156 Deg. C per count and 0 Deg.C = = 131.5 Counts */ { Mode = 4; /* 55/0.5156 + 131 = 238 counts for 55 Deg. C */ } if (Mode != OldMode) { NewMode = 1; /* flag for new mode*/ } if ((Mode == 4) && ((SetPtCurrent0 != 0) || (SetPtCurrent1 != 0)) ) /* if fail zero out setpoints*/ { SetPtCurrent0 = 0; SetPtCurrent1 = 0; NewSetPoint = 1; } } /************************************************************************************/ /******************************* Write All Output ***********************************/ /************************************************************************************/ /*>>>>>>>working here >>>>>>*/ void WriteAllOutput (void) { /********************** Set DAC per New set point **************************/ if (NewSetPoint == 1) /* Update DAC voltage and current set pionts*/ { DAC0_CS_L = 0; /* select DAC 1KW Voltage set point*/ WriteSPI(HiSetPtCurrent0); /*Set DAC A to 0*/ WriteSPI(LoSetPtCurrent0); /*Set DAC A to 0 */ DAC0_CS_L = 1; /* Deselect DAC */ DAC0_CS_L = 0; /* select DAC */ WriteSPI(HiSetPtCurrent1 | 0x10); /*Set bit 4 of high byte to address DAC B Current set point*/ WriteSPI(LoSetPtCurrent1); /*Set DAC B to 0*/ DAC0_CS_L = 1; /* Deselect DAC */ DAC1_CS_L = 0; /* select DAC set point*/ WriteSPI(HiSetPtProbe); /*Set DAC A to 0*/ WriteSPI(LoSetPtProbe); /*Set DAC A to 0 */ DAC1_CS_L = 1; /* Deselect DAC */ DAC1_CS_L = 0; /* select DAC */ WriteSPI(HiSetPtBrake | 0x10); /*Set bit 4 of high byte to address DAC B Current set point*/ WriteSPI(LoSetPtBrake); /*Set DAC B to 0*/ DAC1_CS_L = 1; /* Deselect DAC */ NewSetPoint = 0; /*Clear New set point*/ } /********************* Set power supplies per Mode ************************/ if ( NewMode ) { if (Mode == docked_nocharge) { /* Set power supply bits*/ PC104_ON = on; /* Power to PC104, fan and RF ETHERNET */ MOD_USBL_ON = off; /* Power to ACOUSTIC MODEM and USBL */ PWROUT_ON = off; /* Power Output */ CURRENTMETER_ON = off; /* Power Current Meter*/ BRK_ON = off; /* Brake control*/ BRK_FWR_REV_L = off; /* Brake control*/ /* RS232 nib = mode */ RS232CTR3 = 0; /* Set RS232 control nib to AUV Mission */ RS232CTR2 = 0; /* Set RS232 control nib to AUV Mission */ RS232_CTR1 = 0; /* Set RS232 control nib to AUV Mission */ RS232_CTR0 = 1; /* Set RS232 control nib to AUV Mission */ if (ProbeState == 0) { PROBE_ON = off; /* Power supply circuits for Probe*/ } else { PROBE_ON = on; /* Power supply circuits for Probe*/ } } if (Mode == docked_charge) { /* Set power supply bits*/ PC104_ON = on; /* Power to PC104, fan and RF ETHERNET */ MOD_USBL_ON = off; /* Power to ACOUSTIC MODEM and USBL */ PWROUT_ON = on; /* Power Output */ PROBE_ON = off; /* Power Probe*/ CURRENTMETER_ON = off; /* Power Current Meter*/ BRK_ON = off; /* Brake control*/ BRK_FWR_REV_L = off; /* Brake control*/ /* RS232 nib = mode */ RS232CTR3 = 0; /* Set RS232 control nib to AUV Mission */ RS232CTR2 = 0; /* Set RS232 control nib to AUV Mission */ RS232_CTR1 = 0; /* Set RS232 control nib to AUV Mission */ RS232_CTR0 = 1; /* Set RS232 control nib to AUV Mission */ } if (Mode == idle) { /* Set power supply bits*/ PC104_ON = on; /* Power to PC104, fan and RF ETHERNET */ MOD_USBL_ON = on; /* Power to ACOUSTIC MODEM and USBL */ PWROUT_ON = off; /* Power Output */ PROBE_ON = off; /* Power Probe*/ CURRENTMETER_ON = off; /* Power Current Meter*/ BRK_ON = off; /* Brake control*/ BRK_FWR_REV_L = off; /* Brake control*/ /* RS232 nib = mode */ RS232CTR3 = 0; /* Set RS232 control nib to AUV Mission */ RS232CTR2 = 0; /* Set RS232 control nib to AUV Mission */ RS232_CTR1 = 0; /* Set RS232 control nib to AUV Mission */ RS232_CTR0 = 1; /* Set RS232 control nib to AUV Mission */ } if (Mode == auv_returning) { /* Set power supply bits*/ PC104_ON = on; /* Power to PC104, fan and RF ETHERNET */ MOD_USBL_ON = on; /* Power to ACOUSTIC MODEM and USBL */ PWROUT_ON = off; /* Power Output */ PROBE_ON = off; /* Power Probe*/ CURRENTMETER_ON = on; /* Power Current Meter*/ BRK_ON = on; /* Brake control*/ BRK_FWR_REV_L = off; /* Brake control*/ /* RS232 nib = mode */ RS232CTR3 = 0; /* Set RS232 control nib to AUV Mission */ RS232CTR2 = 0; /* Set RS232 control nib to AUV Mission */ RS232_CTR1 = 0; /* Set RS232 control nib to AUV Mission */ RS232_CTR0 = 1; /* Set RS232 control nib to AUV Mission */ } if (Mode == failed_docked) { /* Set power supply bits*/ PC104_ON = on; /* Power to PC104, fan and RF ETHERNET */ MOD_USBL_ON = off; /* Power to ACOUSTIC MODEM and USBL */ PWROUT_ON = off; /* Power Output */ PROBE_ON = off; /* Power Probe*/ CURRENTMETER_ON = on; /* Power Current Meter*/ BRK_ON = on; /* Brake control*/ BRK_FWR_REV_L = off; /* Brake control*/ /* RS232 nib = mode */ RS232CTR3 = 0; /* Set RS232 control nib to AUV Mission */ RS232CTR2 = 0; /* Set RS232 control nib to AUV Mission */ RS232_CTR1 = 0; /* Set RS232 control nib to AUV Mission */ RS232_CTR0 = 1; /* Set RS232 control nib to AUV Mission */ } if (Mode == failed_nondocked) { /* Set power supply bits*/ PC104_ON = on; /* Power to PC104, fan and RF ETHERNET */ MOD_USBL_ON = off; /* Power to ACOUSTIC MODEM and USBL */ PWROUT_ON = off; /* Power Output */ PROBE_ON = off; /* Power Probe*/ CURRENTMETER_ON = on; /* Power Current Meter*/ BRK_ON = on; /* Brake control*/ BRK_FWR_REV_L = off; /* Brake control*/ /* RS232 nib = mode */ RS232CTR3 = 0; /* Set RS232 control nib to AUV Mission */ RS232CTR2 = 0; /* Set RS232 control nib to AUV Mission */ RS232_CTR1 = 0; /* Set RS232 control nib to AUV Mission */ RS232_CTR0 = 1; /* Set RS232 control nib to AUV Mission */ } NewMode = 0; } if (RS232_INT) { /* RS232 nib = mode */ RS232CTR3 = 1; /* Set RS232 control nib to AUV Mission */ RS232CTR2 = 0; /* Set RS232 control nib to AUV Mission */ RS232_CTR1 = 0; /* Set RS232 control nib to AUV Mission */ RS232_CTR0 = 0; /* Set RS232 control nib to AUV Mission */ } } /************************************************************************************/ /********************************** Functions ***************************************/ /************************************************************************************/ void ReadStrTimeOut(char Wait) /* Read a string form the USART, return nul if no data or e1 if time out*/ { if (DataRdyUSART() || (Wait == 'W')) { char i = 0; /* string pointer, exit if 21*/ while (i != 21) { INTCONbits.TMR0IF = 0; /* Clear Timer0 over flow bit*/ WriteTimer0(2^16-31250); /*Set time out to 8 sec (256uSec per count)*/ while ((!INTCONbits.TMR0IF) && (!DataRdyUSART())); /*wait for byte or time out*/ if (!INTCONbits.TMR0IF) /*If Not Time out then data ready*/ { InString[i] = ReadUSART(); /* add USART byte to string*/ if (InString[i] == 13) /* Line feed*/ { InString[i] = '\0'; i = 19; /* now get CR then exit */ if (strlen(InString) == 0) { strcpypgm2ram(InString,"dc"); } } if (InString[i] == 10) /* Carriage return*/ { InString[i] = '\0'; i = 20; /* force exit */ if (strlen(InString) == 0) { strcpypgm2ram(InString,"dc"); } } } else { i = 20; /* 20 + 1 = 21, exit with time out*/ strcpypgm2ram(InString,"e1"); /* Set string to error 1 time out*/ ErrorFlag = 1; } ++i; } } else { strcpypgm2ram(InString,""); /* Set string to null and return*/ } }