/*  BSD-License:

Copyright (c) 2007 by Nils Springob, nicai-systems, Germany
Copyright (c) 2010 by Matthias Bunte, Germany

All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

  * Redistributions of source code must retain the above copyright notice,
    this list of conditions and the following disclaimer.
  * Redistributions in binary form must reproduce the above copyright notice,
    this list of conditions and the following disclaimer in the documentation
    and/or other materials provided with the distribution.
  * Neither the names of the authors the name nicai-systems nor
    the names of its contributors may be used to endorse or promote products
    derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

/*! @file    motorctrl.c
 *  @brief   Controlling the motor speed by pwm
 *  @author  Matthias Bunte (m_bunte@arcor.de)
 *  @date    2010-04-02
 */

#include <stdint.h>
#include <avr/interrupt.h>
#include "iodefs.h"
#if defined(__AVR_ATmega16__)
#include "hal_atmega16_motorctrl.h"
#endif
#include "motorctrl.h"


#define MOTORCTRL_INITSTATE   _BV(0)
#define MOTORCTRL_L_DIR       _BV(1)
#define MOTORCTRL_R_DIR       _BV(2)

#ifdef __cplusplus
extern "C" {
#endif

static uint16_t MotorCtrl_L_Speed;
static uint16_t MotorCtrl_R_Speed;
static uint8_t MotorCtrl_Flags = 0;




/*****************************************************************
**  Neue Geschwindigkeit und Richtung fuer einen Motor vorgeben
*/
static inline void MotorCtrl_SetSpeed( uint8_t IsLeftMotor, uint16_t Speed)
{
   uint8_t sreg_copy;
  
                    // Interruptsicher neue Geschwindigkeit in die PWM-Register schreiben
   sreg_copy = SREG;
   cli();
   if (IsLeftMotor)
   {
      MOTORCTRL_HAL_PWM_REG_L = Speed;
   }
   else
   {
      MOTORCTRL_HAL_PWM_REG_R = Speed;
   }
   SREG = sreg_copy;            // und enable int   
}



/*****************************************************************
** Initialize MCU pins and PWM for motor control
*/
void MotorCtrl_Init(void)
{
                                     // Set all pwm and direction pins to high and as "output"
   set_output_group(IO_MOTOR);
   activate_output_group(IO_MOTOR);
                                     // Init PWM / Timer-Peripherie
   MOTORCTRL_PWM_TIMER_INIT();
                                     // Reset registers to stopp motor
   MOTORCTRL_CHANGE_DIR_IRQ_MASK = 0;
                                     // Alle Geschwindigkeiten auf 0 zuruecksetzen
   MotorCtrl_SetSpeed(MOTORCTRL_RIGHT, MOTORCTRL_STOPP_VAL);
   MotorCtrl_SetSpeed(MOTORCTRL_LEFT, MOTORCTRL_STOPP_VAL);
                                     // Und die intern gespeicherten Geschwindigkeitswerte ebenfalls zuruecksetzen
   MotorCtrl_R_Speed = MOTORCTRL_STOPP_VAL;
   MotorCtrl_L_Speed = MOTORCTRL_STOPP_VAL;
                                     // Das Initialisierungsflag auf 1 setzen, alle anderen loeschen
   MotorCtrl_Flags = MOTORCTRL_INITSTATE;

}



/*****************************************************************
** Initialisierungszustand auslesen
*/
uint8_t MotorCtrl_Initialized(void)
{
   return (MotorCtrl_Flags & MOTORCTRL_INITSTATE);
}



/*****************************************************************
** Lese PWM-Wert eines Motors
*/
uint16_t MotorCtrl_GetSpeed(uint8_t IsLeft)
{
   uint16_t Speed;
  
   if (IsLeft)
   {
      Speed = MotorCtrl_L_Speed;
   }
   else
   {
      Speed = MotorCtrl_R_Speed;
   }
   return Speed;
}



/*****************************************************************
** Neue Geschwindigkeit und Richtung fuer einen Motor vorgeben
*/
void MotorCtrl_SetSpeedDir(uint8_t IsLeftMotor, uint16_t Speed, uint8_t IsBackward)
{
   uint8_t RunningBackward;      // Speichert, ob aktuell Rueckwaerts gefahren wird
   uint8_t DirFlag;              // Speichert, ob das linke oder rechte Richtungs-Flag bearbeitet werden soll

   uint8_t IRQFlag;              // Speichert, 

                                 // Zur Sicherheit: Backward != 0 -> setze es auf den Standardwert
   if (IsBackward)
   {
      IsBackward = MOTORCTRL_BACKWARD;
   }
                                 // Sicherstellen, dass der Maximalwert nicht ueberschritten wird
   if (Speed > MOTORCTRL_FULLSPEED_VAL)
   {
      Speed = MOTORCTRL_FULLSPEED_VAL;
   }
                                 // Sicherstellen, dass der Minimalwert nicht unterschritten wird - ausser 0
   else if ( (Speed != 0) && (Speed < MOTORCTRL_MINSPEED_VAL) )
   {
      Speed = MOTORCTRL_MINSPEED_VAL;
   }
                                 // Wenn LeftMotor gesetzt ist, die neue Geschwindigkeit fuer den linken Motor
                                 // uebernehmen, sonst die Geschwindigkeit fuer den rechten Motor uebernehmen.
   if (IsLeftMotor)
   {
      RunningBackward = ( !get_output_groupbit(IO_MOTOR, L_DIR) )?1:0;
      DirFlag = MOTORCTRL_L_DIR;
      IRQFlag = MOTORCTRL_CHANGE_L_DIR_IRQ;
      MotorCtrl_L_Speed = Speed;
   }
   else
   {
      RunningBackward = get_output_groupbit(IO_MOTOR, R_DIR)?1:0;
      DirFlag = MOTORCTRL_R_DIR;
      IRQFlag = MOTORCTRL_CHANGE_R_DIR_IRQ;
      MotorCtrl_R_Speed = Speed;
   }
                                 // Wenn sich die Richtung nicht geaendert hat, Wert uebernehmen
   if ( (RunningBackward == IsBackward) || (MotorCtrl_GetSpeed(IsLeftMotor) == MOTORCTRL_STOPP_VAL) )
   {
      MotorCtrl_SetSpeed(IsLeftMotor, Speed);
   }
   else
   {
                                // Bei Rueckwaertsfahrt Flag setzen, bei Vorwaertsfahrt Flag loeschen
      if (IsBackward)
      {
         SET_FLAG(MotorCtrl_Flags, DirFlag);
      }
      else
      {
         CLR_FLAG(MotorCtrl_Flags, DirFlag);
      }

                               // Motor anhalten
      MotorCtrl_SetSpeed(IsLeftMotor, MOTORCTRL_STOPP_VAL);
                               // Set IRQ-Mask to enter IRQ on PWM-Timer = 0
                               // At that time the direction will be changed
      SET_FLAG(MOTORCTRL_CHANGE_DIR_IRQ_MASK, IRQFlag);
   }
}



/*****************************************************************
** Stops both motors
*/
void MotorCtrl_Stopp(void)
{
   MotorCtrl_SetSpeedDir(MOTORCTRL_LEFT,  MOTORCTRL_STOPP_VAL, MOTORCTRL_FORWARD);
   MotorCtrl_SetSpeedDir(MOTORCTRL_RIGHT, MOTORCTRL_STOPP_VAL, MOTORCTRL_FORWARD);
}



/*****************************************************************
** Interrupt service routine 
** Set new speed value for left motor after change of direction 
** Check if PWM at motor left is stopped
** The outpin of the ATmega is inverted by 74HC139,
** so check for the pin is set
*/
ISR(TIMER1_COMPA_vect)
{ 
   if (get_input_groupbit(IO_MOTOR, L_PWM))
   {
                                                // Set direction to new value stored in MotorCtrl_Flags 
      set_output_groupbitval( IO_MOTOR, L_DIR, !GET_FLAG(MotorCtrl_Flags, MOTORCTRL_L_DIR) );

      MOTORCTRL_HAL_PWM_REG_L = MotorCtrl_L_Speed;
                                                // Clear IRQ-Mask 
                                                // When changing direction again 
                                                // this IRQ has to be entered again 
      CLR_FLAG(MOTORCTRL_CHANGE_DIR_IRQ_MASK, MOTORCTRL_CHANGE_L_DIR_IRQ);
   }
}



/*****************************************************************
** Interrupt service routine 
**
** Set new speed value for right motor after change of direction 
** Check if PWM at motor right is stopped
** The outpin of the ATmega is inverted by 74HC139,
** so check for the pin is set
*/
ISR(TIMER1_COMPB_vect)
{
   if (get_input_groupbit(IO_MOTOR, R_PWM))
   {
                                 /* Set direction to new value stored in MotorCtrl_Flags */
      set_output_groupbitval( IO_MOTOR, R_DIR, GET_FLAG(MotorCtrl_Flags, MOTORCTRL_R_DIR) );

      MOTORCTRL_HAL_PWM_REG_R = MotorCtrl_R_Speed;
                                                // Clear IRQ-Mask 
                                                // When changing direction again 
                                                // this IRQ has to be entered again 
      CLR_FLAG(MOTORCTRL_CHANGE_DIR_IRQ_MASK, MOTORCTRL_CHANGE_R_DIR_IRQ);
   }
}


#ifdef __cplusplus
} // extern "C"
#endif


