/* ANKS2 Anklappssteuerung fuer Aussenspiegel Version II               */
/*                       __    _                                       */                     
/*            __________/ /_  (_)________   ___  __  __                */       
/*           / ___/ ___/ __ \/ / ___/ __ \ / _ \/ / / /                */ 
/*          (__  ) /__/ / / / / /  / /_/ //  __/ /_/ /                 */ 
/*         /____/\___/_/ /_/_/_/  / .___(_)___/\__,_/                  */ 
/*                               /_/                                   */ 
/*                                                                     */ 
/* (c) C.Schirp released under the creative commons BY-NC-SA license   */
/*     for further details see                                         */ 
/*     http://creativecommons.org/licenses/by-nc-sa/3.0/               */ 
/*     schirp.eu must be named in derived software coyright notes      */
/*                                                                     */
/* V1.0 Erstausgabe                                                    */
/* V1.1 Änderung der Unterspannungserkennung HW1.3 erforderlich        */
/* V1.2 Entprellen der Eingangsinale, schneller Wechsel in Sleep       */
/* V2.0 Ausgabe fuer 16F688 und Leiterplatte ANKS2 V1.0                */
/* V2.1 Erweiterung um Speedlock-Schutz via KL15 an INP_C              */
/*                                                                     */



#include <c:\projekte\picsel\cc5\16F688.h>
#include <c:\projekte\picsel\cc5\int16CXX.h>
#include <c:\projekte\picsel\cc5\inline.h>

#pragma config = 0x00F4 // 0000.0000.1111.0100 
// INTIO2

// Version mit 2 oder 3 Inputs
//#define INP_2
#define INP_3


/* Port- Defines */

#define TR4_2    0b.0000.0100	// RC2 Transistor 4-2 (L) ausklappen
#define TR4_1    0b.0000.1000	// RC3 Transistor 4-1 (L) anklappen
#define TR3_2    0b.0010.0000	// RC5 Transistor 3-2 (H) ausklappen
#define TR3_1    0b.0001.0000	// RC4 Transistor 3-1 (H) anklappen

#define IN_UNTER  0b.0000.0001	// RC0 Unterspannungssignal IN
#define PU_UNTER  0b.0000.0010	// RC1 Unterspannungssignal Pullup


#define INP_A   0b.0001.0000	// RA4 Eingang A
#define INP_B   0b.0010.0000	// RA5 Eingang B
#define INP_C   0b.0000.0010	// RA1 Eingang C
#define INP_AB  0b.0011.0000	// RA Eingang A+B
#define INP_ABC 0b.0011.0010	// RA alle Eingaenge


/* Programmkonstanten */
#define MOTORSTROM_H	80	// 1,2A x 0,33R
#define MOTORSTROM_L	5	//
#define MOTORZEIT_H	400	// Max laufzeit in 10ms-Schritten

#define	ST_IDLE		0	// Stati-Defines
#define ST_ANKLAPP	1
#define ST_AUSKLAPP	2

#define END_STROM	0xFF00	// Werte für Abbruch eines Klappvorganges in main()
#define END_ZEIT	0xFE00
#define END_RICHTUNG	0xFD00
#define END_SLEEP	0xDC00


#define PWM_MAX  100		// Max-Tastgrad/Auflösung fuer Soft-PWM



/* Funktionsprototypen */
void Eingang_testen(void);

/* Variablen */
uns8 dummy, dummy2;	// Hilfsvariablen

uns8 Motorstrom;	// Ergebnis des Medianfilters
uns8 med_a;		// Filterwerte
uns8 med_b;
uns8 med_c;

uns8 soll_status;	// Resultat der Eingangspegelauswertung
uns8 result;		// Zwischenergebnis der Eingangsauswertung
uns8 i;			// for-index fuer Verzoegerung in Eingangsauswertung

uns16 laufzeit;		// Zähler für die Laufzeitüberwachung in main()

uns8 takt, cyc, duty;	// Fuer Soft-PWM


/*------------------------------------------------------------------*/
/*   II  N   N TTTTT                                                */
/*   II  NN  N   T                                                  */
/*   II  N N N   T                                                  */
/*   II  N  NN   T                                                  */
/*   II  N   N   T                                                  */
/*------------------------------------------------------------------*/
#pragma origin 4
interrupt int_server(void)
{
	int_save_registers
	
	// wenn int-on-change, dann status auswerten
	Eingang_testen();
	
	
	int_restore_registers
}



/*------------------------------------------------------------------*/
/*   II  N   N  II TTTTT                                            */
/*   II  NN  N  II   T                                              */
/*   II  N N N  II   T                                              */
/*   II  N  NN  II   T                                              */
/*   II  N   N  II   T                                              */
/*------------------------------------------------------------------*/

void init (void)
{
	/* Oszillatorblock */
	
	OSCCON = 0b0110.0000;	// 4MHz Clock - wie 16F628 mit INTRC
	
	/* PORT A,C Config */
	PORTA =  0x00;	

#ifdef INP_3
	TRISA =  0b0011.1111;	// RA0-5 input  
	
	WPUA= 	 0b0011.0010;	// Pullup an Input A,B,C
	IOCA= 	 0b0011.0010;	// Int on Change an Input A,B,C
#endif

#ifdef INP_2
	TRISA =  0b0011.1100;	// RA0-5 input  RA1 bei einfacher Version als OUTPUT!!!!
	
	WPUA= 	 0b0011.0000;	// Pullup an Input A,B
	IOCA= 	 0b0011.0000;	// Int on Change an Input A,B
#endif

		
	ANSEL =  0b0000.0100;	// AN2/RA2 ist Analog input
	CMCON0 = 0x07;		// Komparatoren aus, alles Digital-I/O
		
	PORTC =  0b0000.0000;	//
	TRISC =  0b0000.0001; 	// RC0 input

	/* ADC */
	ADCON0=	 0b1000.1001;	// AD-Wandler rechtsbuendig, VDD=ref,  Channel AN2, Stopped, AD_ON
	ADCON1 = 0b0001.0000;	// Fosc/8,

	/*sonstiges*/
	T1CON = 0x00;		// Timer disabled
	OPTION_REG = 0x04;	/* pull-up porta, falling edge int, FOSC/4 source, no WDT prescaler, prescale=32 */
	INTCON = 0b.0000.1000; 	// int disabled und gelöscht nur Int-on-change aktiv

	/* Variablen initialisieren */

	med_a = MOTORSTROM_L;
	med_b = MOTORSTROM_L;
	med_c = MOTORSTROM_L;
	
	soll_status = ST_IDLE;	
	
}

/*------------------------------------------------------------------*/
/*                                                                  */
/*                                                                  */
/*                                                                  */
/*                                                                  */
/*                                                                  */
/*------------------------------------------------------------------*/

//
// Pause für ms Milisekunden
//
void Delay1ms(uns16 ms)
{
	while(ms)				// Schleife verlassen wen ms=0 ist
	{
	    OPTION = 2; 		// Vorteiler auf 8 einstellen
	    TMR0 = 131; 		// 125 * 8 = 1000 (= 1 ms)
	    while (TMR0);		// abwarten einer Milisekunde
	    ms--;				// "ms" mit jeder Milisekunde ernidrigen
	}
}
void Delay100u(uns16 us)
{
#pragma updateBank 0 /* OFF, all cores */

uns8 count;
 do
 {
 	us--;	
	for (count = 0; count < 0x10; count++) // F0
	{
		NOP;	
	}
}
while (us);

#pragma updateBank 1 /* ON, all cores */
}


//
// 3-Tap Medianfilter für die Motorstromwerte
// ältestes Element ist a, neuestes ist c
//
void MedianFilter(void)
{
#pragma updateBank 0 /* OFF, all cores */

  // Medianfilter 

  if (med_a>med_b)
   if (med_a<med_c)
    Motorstrom = med_a;
   else
    if (med_b<med_c)
     Motorstrom = med_c;
    else
     Motorstrom = med_b;
  else
   if (med_a>med_c)
    Motorstrom = med_a;
   else
    if (med_b<med_c)
     Motorstrom = med_b;
    else
     Motorstrom = med_c;
     
   
   // Werteliste weiterschieben
   med_a = med_b;
   med_b = med_c;
     
#pragma updateBank 1 /* OFF, all cores */
}

//
// Testen der Eingänge INP_A, INP_B und UNTER
// setzen des Soll-Modus
//
void Eingang_testen(void)
{

	// hier etwas warten
	for (i = 0; i <200; i++)
	{
		nop();
	}
	
	result = PORTA;		// Port lesen
	RBIF = 0;	// Int on change-Request loeschen

#ifdef INP_2	
	result &= INP_AB;	// Eingänge maskieren
#endif

#ifdef INP_3
	
	// wenn INP_C = high, dann ist KL15=an -> es wird nicht angeklappt
	if (result & INP_C)
	{	
		result = 0x00;	
	}
	else
	{
		// INP_C = KL15 = LOW -> dann wird bei Bedarf geklappt
		
		result &= INP_AB;	// Eingänge maskieren
	}
#endif
		
	switch (result)
	{
		case INP_A: soll_status = ST_AUSKLAPP; break;
		case INP_B: soll_status = ST_ANKLAPP; break;
		default: break;
	}
}

//
// Sleepmode-Entry mit Abschalten der Peripherie
// und Rücksetzen der Portchange-Bedingungen
//
void goto_sleep(void)
{
	
	dummy = PORTA;	// Porta lesen fuer Reset des Wakeup on change
	RBIF = 0;	// Portchange-Request loeschen
	GIE = 0;	// Global Int sperren
	
	ADON = 0; 	// ADC ausschalten
	sleep();
	
	RBIF = 0;	// Portchange-Request loeschen
	GIE = 1;	// Global Int freigeben	
	
	// Todo: entweder hier etwas warten...oder in eingang_testen()
}

//
// Diagnose der Brueckenfehler Short to Bat und Short to GND
// wird 3x mit je 1ms Abstand abgefragt, bei 2x Faultcondition wird Fehler gesetzt
//

uns8 diagnose(void)
{
	
	uns8 ret_wert = 1; // Returnwert fuer OKAY
	
	uns8 counter;
	
	// Faelle B&C: Batterieschluss
	// Teste, ob ohne Highside-Transistoren schon Spannung anliegt
	// Pullup für Kompartor an
	PORTC = PU_UNTER;
	Delay1ms(1);	// 1ms warten
	
	counter = 0;
	
	if (PORTC & IN_UNTER)
		counter++;
	
	Delay1ms(1);	// 1ms warten
	
	if (PORTC & IN_UNTER)
		counter++;
	
	Delay1ms(1);	// 1ms warten
		
	if (PORTC & IN_UNTER)
		counter++;
		
	
	
	if (counter > 1)
	{
		// es liegt schon Spannung an -> FEHLER!
		ret_wert = 0;	
	}
	else
	{
		// Soweit okay, jetzt Faelle D&E: Masseschluss
		PORTC = TR3_1 + PU_UNTER; 
		Delay100u(1);	// 1ms warten
		
		
		counter = 0;
	
		if (!(PORTC & IN_UNTER))
			counter++;
		
		Delay100u(1);	// 1ms warten
		
		if (!(PORTC & IN_UNTER))
			counter++;
		
		Delay100u(1);	// 1ms warten
			
		if (!(PORTC & IN_UNTER))
			counter++;
		
		if (counter > 1)
		{
			// es liegt keine Spannung an -> FEHLER!
			ret_wert = 0;	
		}
		
	}
	PORTC = 0;
	Delay1ms(1);
	
	return (ret_wert);
		
}

//
// Soft-PWM fuer rechts- und linkslauf
// Portmuster übergeben in dummy und dummy2
//
void do_pwm(void)
{
	GIE = 0;	// Global Int sperren
	
	for (takt = 0; takt<PWM_MAX; takt++)
	{
		duty = PWM_MAX-takt;
	
		PORTC &= dummy;	// Off
	
		for ( cyc = 0; cyc < duty; cyc++)
		{
			nop();
		}
	
		PORTC |= dummy2;	//On
		
		for ( cyc = duty; cyc < PWM_MAX; cyc++)
		{
			nop();
		}
	
	}
	GIE = 1;	// Global Int freigeben
}

//
// Start des Anklappvorganges, Aufruf der Soft-PWM
// Portmuster werden übergeben in dummy und dummy2
//
void anklappen(void)
{
	if (diagnose())
	{	
		
		// erst Highside  an
		PORTC = TR3_1;
		Delay1ms(1);	// 1ms warten
		
		//
		// PWM-Rampe mit steigender ON-Zeit 0..100
		//
		dummy = 0xff - TR4_1; 	// Off
		dummy2 = TR4_1;		// On
		
		do_pwm();
		laufzeit = 0;
	}
	else
	{
		laufzeit = END_SLEEP; // Motor nicht stoppen, direkt schlafenlegen wg. Fehler
	}
	

}

//
// Start des Ausklappvorganges, Aufruf der Soft-PWM
// Portmuster werden übergeben in dummy und dummy2
//
void ausklappen(void)
{
	if (diagnose())
	{
		
		// erst Highside an, dann Lowside
		PORTC = TR3_2;
		Delay1ms(1);	// 1ms warten
		
		//
		// PWM-Rampe mit steigender ON-Zeit 0..100
		//
		
		dummy = 0xff - TR4_2;	// off
		dummy2 = TR4_2;		// on
		
		do_pwm();
		laufzeit = 0;
	}
	else
	{
		laufzeit = END_SLEEP; // Motor nicht stoppen, direkt schlafenlegen wg. Fehler
	}	

}

//
// Motor-Stopp mit beiden Lowside-Treibern
// Reset der Motorstrom-Filterwerte
//
void motor_stop(void)
{
	PORTC = 0;	// Alle Treiber und Pullup aus
	Delay1ms(1);	// 1ms warten
	
	PORTC = TR4_2 + TR4_1;	// Lowside-Treiber beide an
	Delay1ms(100);
	PORTC = 0;	// Alle Treiber aus
	
	med_a = MOTORSTROM_L;	// Filterwerte loeschen
	med_c = MOTORSTROM_L;	
	med_b = MOTORSTROM_L;
	
}

//
// Strommessung über ADC-Eingang AN2
// AD-Wert wird in das Filterelement C geschrieben
//
void strom_messen(void)
{
		
	ADCON0=	 0b1000.1001;	// AD-Wandler rechtsbuendig, VDD=ref,  Channel AN2, Stopped, AD_ON
	GO = 1;			// Wandlung starten
	
	while (GO);		// Auf ADC warten
	
	// Overflow abfangen
	if (ADRESH)
	{
		med_c = 0xFF;	
	}
	else
	{
		med_c = ADRESL;
	}
		
}




/*------------------------------------------------------------------*/
/*     M     M    A    II  N   N                                    */
/*     MM   MM   A A   II  NN  N                                    */
/*     M M M M  A   A  II  N N N                                    */
/*     M  M  M  AAAAA  II  N  NN                                    */
/*     M     M  A   A  II  N   N                                    */
/*------------------------------------------------------------------*/

void main (void)
{

	uns8 akt_status;
		
	init();
	
		
	while (1)
	{
		
		// wenn nichts zu tun
		if (soll_status == ST_IDLE)
		{
			goto_sleep();	// Gehe schlafen
		
			Eingang_testen(); // teste Eingangspegel
		}
		
		
		if (soll_status != ST_IDLE)
		{
			laufzeit = END_SLEEP;	// default: sofort wieder einschlafen
			
			if (soll_status == ST_ANKLAPP)
				Delay1ms(5000); // 5s warten (wegen Fehlbedienung)
			
			if (soll_status == ST_AUSKLAPP)
			{
				// dann ausklappen
				ausklappen();
			}
			else if (soll_status == ST_ANKLAPP)
			{
				// ansonsten anklappen
				anklappen();
			}
						
			akt_status = soll_status;
			
			while (laufzeit < MOTORZEIT_H)
			{
				
				laufzeit++;	// Zeitscheiben zaehlen
				
				strom_messen(); // Motorstrom messen
				
				MedianFilter();	// Filtern
				
				// Endabschaltung?
				if (Motorstrom > MOTORSTROM_H)
					laufzeit = END_STROM;
					
				// Kein Strom koennte Leerlauf oder short to GND sein..: abschalten!	
				if (Motorstrom < MOTORSTROM_L)
					laufzeit = END_STROM;
				
				// hat sich das klappziel geändert?
				if (akt_status != soll_status)
					laufzeit = END_RICHTUNG;
					
				// nur warten, wenn Motor nicht gestoppt werden soll
				if (laufzeit < MOTORZEIT_H)
					Delay1ms(10);	// warten 0,1s
				
			} // von while (laufzeit)
			
			// wenn Motor tatsächlich bewegt wurde, stoppen
			if (laufzeit != END_SLEEP)
				motor_stop();
			
			// bei Überstom und -zeit klappvorgang beenden
			if (laufzeit != END_RICHTUNG)
				soll_status = ST_IDLE;
			
			
		} // von if != IDLE
		
	}; // von while(immer)


}