/*
 * c't-Sim - Robotersimulator fuer den c't-Bot
 * 
 * This program is free software; you can redistribute it
 * and/or modify it under the terms of the GNU General
 * Public License as published by the Free Software
 * Foundation; either version 2 of the License, or (at your
 * option) any later version. 
 * This program is distributed in the hope that it will be 
 * useful, but WITHOUT ANY WARRANTY; without even the implied
 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 
 * PURPOSE. See the GNU General Public License for more details.
 * You should have received a copy of the GNU General Public 
 * License along with this program; if not, write to the Free 
 * Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 * MA 02111-1307, USA.
 * 
 */

/*! @file 	motor.c
 * @brief 	High-Level-Routinen fuer die Motorsteuerung des c't-Bot
 * @author 	Benjamin Benz (bbe@heise.de)
 * @date 	15.01.05
*/

#include <stdlib.h>
#include "global.h"
#include "ct-Bot.h"
#include "motor.h"
#include "bot-local.h"
#include "motor-low.h"
#include "timer.h"
#include "sensor.h"
#include "display.h"

int16 speed_l=0;	/*!< Geschwindigkeit linker Motor */
int16 speed_r=0;	/*!< Geschwindigkeit rechter Motor */

direction_t direction;		/*!< Drehrichtung der Motoren */

#define PWMMAX 255   /*!< Maximaler PWM-Wert */
#define PWMMIN 0     /*!< Minimaler PWM-Wert */

#ifdef SPEED_CONTROL_AVAILABLE
	int8 encoderTargetRateL=0;
	int8 encoderTargetRateR=0;

	/*!
	 * @brief 	Drehzahlregelung fuer die Motoren des c't-Bots
	 * @author 	Benjamin Benz (bbe@heise.de
	 * @date 	01.05.06
	 * Getrennte Drehzahlregelung fuer linken und rechten Motor sorgt fuer konstante Drehzahl und somit annaehernd
	 * fuer Geradeauslauf
	 * Feintuning von Kp, Ki, Kd verbessert die Genauigkeit und Schnelligkeit der Regelung
	 * Querkopplung der Motoren verbessert Gleichlauf, beispielsweise x*(sensEncL - sensEncR) 
	 * in jeden Regler einbauen
	*/
	void speed_control(void){
		int8 Kp=0;
		int8 Ki=0;
	
		int16 StellwertL=motor_left;  /*!< Stellwert links*/
		int16 StellwertR=motor_right;  /*!< Stellwert rechts*/
		
		static int16 lastEncoderL=0;      /*!< vorhergehender Wert von sensEncL */	
		static int8 lastErrL=0;   /*!< letzter Drehzahlfehler links */
		static int8 last2ErrL=0;  /*!< vorletzter Drehzahlfehler links */
	
		static int16 lastEncoderR=0;      /*!< vorhergehender Wert von sensEncR */	
		static int8 lastErrR=0;   /*!< letzter Drehzahlfehler rechts */
		static int8 last2ErrR=0;  /*!< vorletzter Drehzahlfehler rechts */
	
		int16 err=0;    	        // aktuelle Abweichung vom Soll-Wert
		int16 encoderRate=0;		// IST-Wert [Encoder-ticks/Aufruf]
	
	
		// Wir arbeiten mit verschiedenen PID-Parametern fuer verschiedene Geschwindigkeitsabschnitte
		if (encoderTargetRateL <= PID_LOW_RATE){
			Kp=PID_LOW_Kp;
			Ki=PID_LOW_Ki;
		} else {
			if (encoderTargetRateL >= PID_HIGH_RATE) {
				Kp=PID_HIGH_Kp;
				Ki=PID_HIGH_Ki;
			} else {
				Kp=(PID_HIGH_Kp+PID_LOW_Kp)/2;
				Ki=(PID_HIGH_Ki+PID_LOW_Ki)/2;
			}
		}
	
	
	
		//Regler links  
		if (encoderTargetRateL == 0){
			StellwertL=0;
			err=0; lastErrL = 0;
		} else {
			encoderRate = sensEncL-lastEncoderL;  	 // aktuelle Ist-Wert berechnen [Encoder-ticks/aufruf]
			lastEncoderL = sensEncL;   				 // Anzahl der Encoderpulse merken fuer naechsten Aufruf merken
			err = encoderTargetRateL - encoderRate;  // Regelabweichung links
	
			// Stellwert Berechnen
			StellwertL +=  (Kp * (err - lastErrL)) / 10;				// P-Beitrag
			StellwertL +=  (Ki * (err + lastErrL)/2 ) /10;			// I-Beitrag
		//	StellwertL +=  Kd * (errL - 2 * lastErrL + last2ErrL);	// D-Beitrag
			
			//berechneten Stellwert auf zulaessige Groesse begrenzen  
			if (StellwertL >  PWMMAX)	StellwertL =  PWMMAX;         
			if (StellwertL < -PWMMAX)	StellwertL = -PWMMAX; 
		
			#ifdef DISPLAY_REGELUNG_AVAILABLE
				if (display_screen==DISPLAY_REGELUNG_AVAILABLE){
					display_cursor(1,1);
					display_printf("%03d/%03d ",encoderRate,encoderTargetRateL);
					display_cursor(2,1);
					display_printf("e =%03d ",err);
					display_cursor(3,1);	
					display_printf("L =%04d ", StellwertL);
				}
			#endif
		}
	
		last2ErrL = lastErrL;              // alten N-2 Fehler merken
		lastErrL = err;                   // alten N-1 Fehler merken
	
	
		//Regler rechts  
		if (encoderTargetRateR == 0){
			StellwertR=0;
			err=0; lastErrR=0;
		} else {
			encoderRate = sensEncR-lastEncoderR;  	// aktuelle Ist-Wert berechnen [Encoder-ticks/aufruf]
			lastEncoderR = sensEncR;   				// Anzahl der Encoderpulse merken fuer naechsten Aufruf merken
			err = encoderTargetRateR - encoderRate;  // Regelabweichung links
			  
			// Stellwert Berechnen
			StellwertR +=  (Kp * (err - lastErrR))/10;					// P-Beitrag
			StellwertR +=  (Ki * (err + lastErrR)/2)/10;					// I-Beitrag
		//	StellwertR +=  Kd * (err - 2 * lastErrR + last2ErrR);	// D-Beitrag
			
			//berechneten Stellwert auf zulaessige Groesse begrenzen  
			if (StellwertR >  PWMMAX) StellwertR =  PWMMAX;         
			if (StellwertR < -PWMMAX) StellwertR = -PWMMAX;
	
			#ifdef DISPLAY_REGELUNG_AVAILABLE
				if (display_screen==DISPLAY_REGELUNG_AVAILABLE){
					display_cursor(1,10);
					display_printf("%03d/%03d ",encoderRate,encoderTargetRateR);
					display_cursor(2,10);
					display_printf("e =%03d ",err);
					display_cursor(3,10);	
					display_printf("R =%04d ", StellwertR);	
				}
			#endif
		}
	
		last2ErrR = lastErrR;              // alten N-2 Fehler merken
		lastErrR = err;                   // alten N-1 Fehler merken
	
		#ifdef DISPLAY_REGELUNG_AVAILABLE
			if (display_screen==DISPLAY_REGELUNG_AVAILABLE){
				display_cursor(4,1);	
				display_printf("Kp=%03d Ki=%03d", Kp, Ki);	
			}
		#endif
	
		// Und nun den Wert setzen
		bot_motor(StellwertL,StellwertR);
	}
#endif

/*!
 * Direkter Zugriff auf den Motor
 * @param left	Geschwindigkeit fuer den linken Motor
 * @param right Geschwindigkeit fuer den linken Motor
 * Geschwindigkeit liegt zwischen -255 und +255.
 * 0 bedeutet Stillstand, 255 volle Kraft voraus, -255 volle Kraft zurueck.
 * Sinnvoll ist die Verwendung der Konstanten: BOT_SPEED_XXX, 
 * also z.B. motor_set(BOT_SPEED_LOW,-BOT_SPEED_LOW);
 * fuer eine langsame Drehung
*/
void motor_set(int16 left, int16 right){
	#ifdef SPEED_CONTROL_AVAILABLE
		static int16 old_mot_ticks=0;
	#endif

	if (left == BOT_SPEED_IGNORE)	
		left=BOT_SPEED_STOP;

	if (right == BOT_SPEED_IGNORE)	
		right=BOT_SPEED_STOP;

	
	// Haben wir ueberhaupt etwas zu tun?
	if ((speed_l == left) && (speed_r == right)){
		// Hier sitzt die eigentliche Regelung
		#ifdef SPEED_CONTROL_AVAILABLE
			register uint16 ticks = TIMER_GET_TICKCOUNT_16;
			if (ticks-old_mot_ticks > MS_TO_TICKS((uint16)SPEED_CONTROL_INTERVAL)) {
				speed_control();
				old_mot_ticks = TIMER_GET_TICKCOUNT_16;
			}
		#endif
		return;		// Keine Aenderung? Dann zuerueck
	}
		
	if (abs(left) > BOT_SPEED_MAX)	// Nicht schneller fahren als moeglich
		speed_l = BOT_SPEED_MAX;
	else if (left == 0)				// Stop wird nicht veraendert
		speed_l = BOT_SPEED_STOP;
	else if (abs(left) < BOT_SPEED_SLOW)	// Nicht langsamer als die 
		speed_l = BOT_SPEED_SLOW;	// Motoren koennen
	else				// Sonst den Wunsch uebernehmen
		speed_l = abs(left);

	if (abs(right) > BOT_SPEED_MAX)// Nicht schneller fahren als moeglich
		speed_r = BOT_SPEED_MAX;
	else if (abs(right) == 0)	// Stop wird nicht veraendert
		speed_r = BOT_SPEED_STOP;
	else if (abs(right) < BOT_SPEED_SLOW)	// Nicht langsamer als die 
		speed_r = BOT_SPEED_SLOW;	// Motoren koennen
	else				// Sonst den Wunsch uebernehmen
		speed_r = abs(right);
	
	if (left < 0 )
		speed_l=-speed_l;
	
	if (right < 0 )	
		speed_r=-speed_r;
		
	#ifdef SPEED_CONTROL_AVAILABLE
		// TODO Hier koennten wir die Motorkennlinie heranziehen um gute Einstiegswerte fuer die Regelung zu haben
		// Evtl. sogar eine im EEPROM kalibrierbare Tabelle??
		encoderTargetRateL = left / SPEED_TO_ENCODER_RATE;	// Geschwindigkeit [mm/s] umrechnen in [EncoderTicks/Aufruf]
		encoderTargetRateR = right / SPEED_TO_ENCODER_RATE;	// Geschwindigkeit [mm/s] umrechnen in [EncoderTicks/Aufruf]
	#endif
	
	// Zuerst einmal eine lineare Kennlinie annehmen
	bot_motor(speed_l/2,speed_r/2);
	
}

/*!
 * Stellt die Servos
 * Sinnvolle Werte liegen zwischen 8 und 16
 * @param servo Nummer des Servos
 * @param servo Zielwert
 */
void servo_set(uint8 servo, uint8 pos){
	if ((servo== SERVO1) && (pos != SERVO_OFF)) {
		if (pos< DOOR_CLOSE)
			pos=DOOR_CLOSE;
		if (pos> DOOR_OPEN)
			pos=DOOR_OPEN;
	}
	servo_low(servo,pos);
}

/*!
 * Initialisiere den Motorkrams
 */
void motor_init(void){
	speed_l=0;
	speed_r=0;
	motor_low_init();
}