/*
 * bkoSegway.c
 *
 * Created: 20/09/2019 10:21:46
 * Author : goldi
 */

/*		Lesezeichen
*
*			INT VEC - S.44
*
*			ADC MUX - S.214
*
*		PIN Definitionen
*
*		A-Register
*
*			PA0 - UBATT		Abfragen der Betteriespannung
*
*			PA1 - MASSE
*
*			PA2 - ADXL-Y	Achsial-Beschleunigung Y-Achse
*
*			PA3 - ADXL-X	Achsial-Beschleunigung X-Achse
*
*			PA4 - GYRO-X	Neigung X-Achse
*
*			PA5 - GYRO-Y	Neigung Y-Achse
*
*			PA6 - POT		Lenkungs Poti
*
*			PA7 - FOOTDETECT	Prüft ob Nutzer auf dem Segway steht
*
*		B-Register
*
*			PB0 - N/A
*
*			PB1 - N/A
*
*			PB2 - LED3		Rot
*
*			PB3 - LED2		Gelb
*
*			PB4 - LED1		Grün
*
*			PB5 - MOSI		SPI
*
*			PB6 - MISO		SPI
*
*			PB7 - SCK		SPI
*
*		C-Register
*
*			PC0 - N/A
*
*			PC1 - N/A
*
*			PC2 - N/A
*
*			PC3 - N/A
*
*			PC4 - N/A
*
*			PC5 - N/A
*
*			PC6 - N/A
*
*			PC7 - N/A
*
*		D-Register
*
*			PD0 - N/A
*
*			PD1 - N/A
*
*			PD2 - CURFLAG	OVERCURRENT FLAG - SOFORT AUSSCHALTEN
*
*			PD3 - N/A
*
*			PD4 - PWM-L
*
*			PD5 - PWM-R
*
*			PD6 - CW/CCW-A	Clock/Counterclockwise Rotation Selector A
*
*			PD7 - CW/CCW-B	Clock/Counterclockwise Rotation Selector B
*
*		Sonstige
*
*			!RES - Reset Pin
*
*			AREF - AREF			Analoge Referenzspannung
*
*			GND - Ground		An Masse
*
*			VCC - Spannung		5V
*
*			AVCC - Spannung		5V Analog Parallelversorgung
*/

//
//	PRE REQ
//
#ifndef F_CPU
#define F_CPU 16000000UL
#endif

//	Konstanten
//		REG A
#define UBATT DDA0
#define ADXL_Y DDA2
#define ADXL_X DDA3
#define GYRO_X DDA4
#define GYRO_Y DDA5
#define POT DDA6
#define FOOTDETECT DDA7
//		REG B
#define LED_3 DDB4
#define LED_2 DDB3
#define LED_1 DDB2
//		REG D
#define CURRFLAG DDD2
#define PWM_L DDD4
#define PWM_R DDD5
#define CW_CCW_A DDD6
#define CW_CCW_B DDD7
//		OTHER
#define PWM_AL OCR1AL
#define PWM_BL OCR1BL
#define CW_CCW_A DDD6
#define CW_CCW_B DDD7


//
//	GLOBAL VAR
//
volatile char buzzer = 0;
volatile long adcSelect = 0;
volatile long adcADXL = 0;
volatile long adcGYRO = 0;
volatile int adcBattery = 0;
volatile int adcSwitch = 0;
volatile int adcPOT = 0;
volatile int adcRocker = 0;
volatile long total_ADXL_GYRO = 0;
volatile long avg_GYRO = 0;
volatile long avg_ADXL = 0;
volatile long avg_BATT = 0;
volatile int driveA = 0;
volatile int driveB = 0;
volatile int RAW_driveA = 0;
volatile int RAW_driveB = 0;
volatile long driveSum = 0;
volatile long buff0 = 0;
volatile long buff1 = 0;
volatile long buff2 = 0;
volatile long buff3 = 0;
volatile long angle = 0;
volatile int Tcount = 0;
volatile long speed = 0;
volatile long steer = 0;
volatile long correction = 0;
volatile long angle_rate = 0;
volatile long balance = 0;
volatile char mode = 0;
volatile char bCount = 0;
volatile long timeout = 0;
volatile char delta = 0;
volatile long rockerSQ = 0;
volatile int zero_Rocker = 0;
volatile int zero_ADXL = 0;
volatile int zero_GYRO = 0;
volatile int Lcount = 0;
volatile int currLimit = 100;
volatile char errNr = 0;
volatile int off_ADXL = 0;

#define ADC_INPUT_FIRST 0
#define ADC_INPUT_LAST 6
#define ADC_VREF_TYPE 0x40
#define DRIVE_LIMIT 20000
#define TOTAL_LOOP 500
#define TOTAL_LOOP_10 50
#define _standby 0
#define _run 1
#define _warmup 2
#define _down 3
#define SAFE_SPEED 5000
#define SAFE_SPEED_N -5000
#define SW_DOWN 50
#define CRITICAL 80
#define MAX_PWM 180
#define MAX_PWM_N -180
#define BATT_DEAD 487000
#define BATT_OK 505000


//
//	PREDEF
//
void get_speedLimit();
long get_ADC(int adcSel);
void algo();
void process();
void set_pwm();
void get_tiltAngle();


//
//	LIB
//
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

//
//	ISR
//
ISR(ADC_vect){
	cli();
	adcSelect = ADC;
	sei();
}

ISR(TIMER0_OVF_vect){
	cli();
	get_tiltAngle();
	get_speedLimit();
	algo();
	process();
	set_pwm();
	TCNT0 = 100;
	sei();
}

//
//	FUNC
//
void get_tiltAngle(){
	adcGYRO = get_ADC(5);
	adcGYRO = 1024 - adcGYRO;

	adcADXL = get_ADC(3);
	adcBattery = get_ADC(0);
	adcRocker = get_ADC(6);
	adcSwitch = get_ADC(7);

	buff0 = total_ADXL_GYRO / TOTAL_LOOP;
	total_ADXL_GYRO = total_ADXL_GYRO - buff0;

	buff0 = adcADXL - zero_ADXL;
	buff0 = buff0 + off_ADXL;

	total_ADXL_GYRO = total_ADXL_GYRO + buff0;

	buff1 = avg_GYRO / TOTAL_LOOP;
	avg_GYRO = avg_GYRO - buff1;

	avg_GYRO = avg_GYRO + adcGYRO;
	buff1 = avg_GYRO / TOTAL_LOOP_10;

	buff0 = adcGYRO * 10;
	buff1 = buff1 - buff0;
	buff1 = buff1 * 35;
	buff1 = buff1 / 100;
	angle_rate = buff1;

	total_ADXL_GYRO = total_ADXL_GYRO + angle_rate;
	angle = total_ADXL_GYRO / TOTAL_LOOP_10;

	buff1 = avg_BATT / TOTAL_LOOP;
	avg_BATT = avg_BATT - buff1;
	avg_BATT = avg_BATT + adcBattery;

	Lcount++;
}

void get_speedLimit(){
	buff1 = 0;
	if(driveSum > 0){
		buff1 =  driveSum - DRIVE_LIMIT;
	}

	buff1 = buff1 / 70;
	off_ADXL = 0;

	if(buff1 > 13){
		buff1 = 13;
	}
	if(buff1 < 0){
		buff1 = 0;
	}

	if(buff1 > 0){
		off_ADXL = buff1;
		buzzer = 2;
	}

	if(buzzer == 0){
		if(avg_BATT > BATT_OK){
			PORTB |= (1 << LED_1);
			PORTB &= ~((1 << LED_2) | (1 << LED_3));
		}
		if(avg_BATT < BATT_OK){
			PORTB |= (1 << LED_2);
			PORTB &= ~((1 << LED_1) | (1 << LED_3));
		}
		if(avg_BATT < BATT_DEAD){
			PORTB |= (1 << LED_3);
			PORTB &= ~((1 << LED_1) | (1 << LED_2));
		}
	}
}

long get_ADC(int adcSel){
	ADMUX &= ~0x1F;
	switch(adcSel){
		case(0):	//Battery
		break;
		case(5):	//GYRO
		ADMUX |= ((1 << MUX2) | (1 << MUX0));
		break;
		case(3):	//ADXL
		ADMUX |= ((1 << MUX1) | (1 << MUX0));
		break;
		case(6):	//Rocker
		ADMUX |= ((1 << MUX2) | (1 << MUX1));
		break;
		case(7):	//Switch
		ADMUX |= ((1 << MUX2) | (1 << MUX1) | (1 << MUX0));
		break;
	}
	ADCSRA |= (1 << ADSC);
	_delay_us(30);
	return adcSelect;
}

void algo(){
	buff0 = angle;
	buff0 = buff0 * 17;
	buff1 = angle_rate * 17;

	balance = buff1 + buff0;

	driveSum = driveSum + balance;

	if(driveSum > 55000){
		driveSum = 55000;
	}
	if(driveSum < -55000){
		driveSum = -55000;
	}

	buff0 = driveSum / 155;
	buff1 = balance / 165;
	speed = buff0 + buff1;
}

void process(){
	Tcount++;
	if(Tcount > 10){
		if(buzzer == 1){
			PORTB |= ((1 << LED_1) | (1 << LED_2) | (1 << LED_3));
		}
		if(buzzer == 2){
			PORTB |= (1 << LED_3);
		}
	}

	if(Tcount > 16){
		if(buzzer > 0){
			PORTB &= ~((1 << LED_1) | (1 << LED_2) | (1 << LED_3));
			bCount++;
		}

		Tcount = 1;

		if(bCount > 10){
			bCount = 0;
			buzzer = 0;
		}
	}

	rockerSQ = zero_Rocker - adcRocker;

	buff1 = driveSum / 20000;
	if(buff1 < 0){
		buff1 = buff1 * -1;
	}

	if(buff1 < 1){buff1 = 1;}

	rockerSQ = rockerSQ / buff1;

	buff1 = driveSum / 2000;
	if(buff1 < 0){
		buff1 = buff1 * -1;
	}

	if(buff2 > 22){
		buff1 = 22;
	}

	buff1 = 27 - buff1;
	buff2 = buff1 * -1;

	if(rockerSQ > buff1){
		rockerSQ = buff1;
	}
	if(rockerSQ < buff2){
		rockerSQ = buff2;
	}

	steer = rockerSQ;

	if((PIND & (1 << PD2)) == 0){
		currLimit = 125;
	}
	if((PIND & (1 << PD2)) == 1){
		if(currLimit < 30000){
			currLimit = currLimit + 1;
		}
		buzzer = 1;
	}

	speed = speed * 125;
	speed = speed / currLimit;

	RAW_driveA = speed - steer;
	RAW_driveB = speed + steer;

	if(mode == _warmup){
		RAW_driveA = 0;
		RAW_driveB = 0;
		speed = 0;
		correction = 0;
		driveSum = 0;
		total_ADXL_GYRO = 0;
		angle = 0;
		if(adcSwitch > SW_DOWN){
			mode = _run;
		}
	}

	if(mode == _run){
		if(adcSwitch > SW_DOWN){
			timeout = 0;
		}

		if(adcSwitch < SW_DOWN){
			if(driveSum < 0){
				if(driveSum < SAFE_SPEED_N){
					buzzer = 1;
					timeout = timeout + 1;
				}
			}

			if(driveSum > 0){
				if(driveSum > SAFE_SPEED){
					buzzer = 1;
					timeout = timeout + 1;
				}
			}

			if(timeout > CRITICAL){
				mode = _down;
			}
		}
	}

	if(mode == _down){
		for(buff1 = 1; buff1 <= 255; buff1++){
		if(RAW_driveA > 0){
			RAW_driveA = RAW_driveA - 1;
		}
		if(RAW_driveA < 0){
			RAW_driveA = RAW_driveA + 1;
		}
		if(RAW_driveB > 0){
			RAW_driveB = RAW_driveB - 1;
		}
		if(RAW_driveB > 0){
			RAW_driveB = RAW_driveB + 1;
		}

		_delay_ms(2);
		set_pwm();
		}
		mode = _standby;
	}
}

void set_pwm(){
	if(RAW_driveA > MAX_PWM){
		 RAW_driveA = MAX_PWM;
	}
	if(RAW_driveA > MAX_PWM_N){
		RAW_driveA = MAX_PWM_N;
	}

	if(RAW_driveA < 0){
		driveA = driveA * -1;
		PORTD |= (1 << CW_CCW_A);
	}
	if(RAW_driveA >= 0){
		driveA = RAW_driveA;
		PORTD &= ~(1 << CW_CCW_A);
	}

	PWM_AL = 255 - driveA;

	if(RAW_driveB > MAX_PWM){
		RAW_driveB = MAX_PWM;
	}
	if(RAW_driveB > MAX_PWM_N){
		RAW_driveB = MAX_PWM_N;
	}

	if(RAW_driveA < 0){
		driveB = driveB * -1;
		PORTD |= (1 << CW_CCW_A);
	}
	if(RAW_driveA >= 0){
		driveB = RAW_driveB;
		PORTD &= ~(1 << CW_CCW_A);
	}

	PWM_BL = driveB;
}

void err_eval(){
	while (1){
		for (buff0 = 1; buff0 <= errNr; buff0++){
			PORTD |= (1 << LED_3);
			_delay_ms(150);
			PORTD &= ~(1 << LED_3);
			_delay_ms(150);
		}
	}
}

void bootup(){
	for (char c = 0; c < 10; c++){
		PORTB |= ((1 << LED_3) | (1 << LED_2) | (1 << LED_1));
		_delay_ms(50);
		PORTB &= ~((1 << LED_3) | (1 << LED_2) | (1 << LED_1));
		_delay_ms(50);
	}
}

void initTimer0(){
	TCNT0 = 100;
	TIMSK |= (1 << TOIE0);
	TCCR0 |= ((1 << CS02) | (1 << CS00) | (1 << ADPS0) | (1 << ADPS1) | (1 << ADPS2));
}

void initTimer1(){
	TCCR1A |= ((1 << COM1A1) | (1 << COM1B1) | (1 << COM1B0) | (1 << WGM10));
	TCCR1B |= (1 << CS10);
	OCR1AL |= 0xff;
}

void initADC(){
	ADMUX |= (1 << REFS0);
	ADCSRA |= (1 << ADEN) | (1 << ADIE);
}

void init(){
	//		DATA DIRECTION
	DDRA &= ~((1 << UBATT) | (1 << ADXL_Y)| (1 << ADXL_X) | (1 << GYRO_X) | (1 << GYRO_Y) | (1 << FOOTDETECT));
	DDRB |= ((1 << LED_3) | (1 << LED_2) | (1 << LED_1));
	DDRD &= ~(1 << CURRFLAG);
	DDRD |= ((1 << PWM_L) | (1 << PWM_R) | (1 << CW_CCW_A) | (1 << CW_CCW_B));
	//		PIN STATE
	PORTB &= ~((1 << LED_3) | (1 << LED_2) | (1 << LED_1));
	DDRD &= ~((1 << PWM_L) | (1 << PWM_R) | (1 << CW_CCW_A) | (1 << CW_CCW_B));
	avg_BATT = BATT_OK * TOTAL_LOOP;

	initADC();
	initTimer0();
	sei();
}

//
//	MAIN
//
int main(void) {
	init();
	bootup();
	adcSwitch = 0;
	for (buff0 = 1; buff0 <= 10; buff0++){
		adcSwitch = adcSwitch + get_ADC(7);
		_delay_ms(1);
	}
	adcSwitch = adcSwitch / 10;
	_delay_ms(100);
	if(adcSwitch < 100){
		errNr = 4;
		err_eval();
	}


	adcRocker = 0;
	for(buff0 = 1; buff0 <= 10; buff0++){
		adcRocker = adcRocker + get_ADC(6);
		_delay_ms(1);
	}
	zero_Rocker = adcRocker / 10;

	_delay_ms(100);


	adcGYRO = 0;
	for(buff0 = 1; buff0 <= 10; buff0++){
		buff1 = get_ADC(5);
		buff1 = 1024 - buff1;
		adcGYRO = adcGYRO + buff1;
		_delay_ms(5);
	}
	zero_GYRO = adcGYRO / 10;
	avg_GYRO = TOTAL_LOOP * zero_GYRO;


	adcADXL = 0;
	for(buff0 = 1; buff0 <= 10; buff0++){
		buff1 = get_ADC(3);
		adcADXL = adcADXL + buff1;
		_delay_ms(5);
	}
	zero_ADXL = adcADXL / 10;

	if(zero_ADXL < 400){
		errNr = 3;
		err_eval();
	}
	if(zero_ADXL > 600){
		errNr = 3;
		err_eval();
	}

	if(zero_ADXL < 350){
		errNr = 2;
		err_eval();
	}
	if(zero_ADXL > 750){
		errNr = 2;
		err_eval();
	}
	sei();

    while (1) {
    }
}