/*
  Pololu_HPMD.cpp - Library for Pololu high power motor driver.
   Created by Carl Morse, Sep 11, 2012.
   
   v0.5 - released on Pololu forum for feedback.
   
 */
//TODO: Performance measurment of functions.

//Refrence of motor driver control pins 
/* 
CS -  ACS714 current sensor output (66 mV/A centered at 2.5 V).

PWMH  -  Pulse width modulation inputs.
PWML -   Pulse width modulation inputs.
DIR - LOW   - Direction input: when DIR is high current will flow from OUTA to OUTB, 
              when it is low current will flow from OUTB to OUTA.
RESET - HIGH - The reset pin, in reset when pulled low.
FF1 - LOW - Fault flag 1 
FF2 - LOW - Fault flag 2 
 
*/
#include "Arduino.h"
#include "pololu_HPMD.h"


 pololu_HPMD::pololu_HPMD(int cs_pin, int PWMH_pin, int PWML_pin, int DIR_pin, int reset_pin, int FF1_pin, int FF2_pin)
    {
        //setup pins
        pinMode(_FF1_pin, INPUT);
        pinMode(_FF2_pin, INPUT);
        pinMode(_reset_pin, OUTPUT);
    
        analogReference(DEFAULT);
        pinMode(_cs_pin, INPUT);
    
        pinMode(_PWMH_pin, OUTPUT);
        pinMode(_PWML_pin, OUTPUT);
        pinMode(_DIR_pin, OUTPUT);
        
        //place public varaibles into a known state.
        _reset = false;
        _ff1 = false;
        _ff2 = false;
        _current = 0;
        
        //place private varibles into a known state.
        _cs =0;
        _PWMH =0;
        _PWML =0;
        _DIR =0;
        _status =0;
    }
    
    void pololu_HPMD::set_reset()
     {
       digitalWrite(_reset_pin, false);
        _reset = true;
        _cs =0;
        _PWMH =0;
        _PWML =0;
        _DIR =0;
       
     }
     
     int pololu_HPMD::get_faults()
     {
         int error =0;
        _ff1 = digitalRead(_FF1_pin);
        _ff2 = digitalRead(_FF2_pin);
        
        if (_ff1 == true ) { error = 1; }
        if (_ff2 == true ) {error = error + 2;}
        return (error);
     }

    void pololu_HPMD::get_status(int arg[])
    {
        int state = 0;
        _DIR = digitalRead(_DIR_pin);
        _PWMH = digitalRead(_PWMH_pin);
        _PWML = digitalRead(_PWML_pin);
        _DIR = digitalRead(_DIR_pin);
        _cs = analogRead(_cs_pin);
        _ff1 = digitalRead(_FF1_pin);
        _ff2 = digitalRead(_FF2_pin);
        
        state = 0;
        if (_ff1 == true) {state = 1;}
        
        if (_ff2 == true) {state  = state + 2;}
        
        if (state  > 0)  {set_reset();}
        
        arg[0] =state;
        arg[1] =_DIR;
        arg[3] = _PWMH;
        arg[4] = _PWML;
        arg[5] = _DIR;
        arg[6] =_cs;
        
    }
     
     
     void pololu_HPMD::start_driver()
     {
        //put motor in a unpowered state same as coast but we are rasing reset allowing the driver to operate
        digitalWrite(_DIR_pin, false);
        digitalWrite(_PWMH_pin, 0);
        digitalWrite(_PWML_pin, 0);
        digitalWrite(_reset_pin, true);
        _reset = false;
     }
    
    void pololu_HPMD::coast()
     {
       digitalWrite(_PWMH_pin, 0);
       digitalWrite(_PWML_pin, 0);
     }
    
    //brake Low
     void pololu_HPMD::braking()
     {
        digitalWrite(_PWMH_pin, 0);
        digitalWrite(_PWML_pin, 255);
     }
   
     void pololu_HPMD::forward(int pwm)
     {
        digitalWrite(_DIR_pin, LOW);
        digitalWrite(_PWMH_pin, pwm);
        digitalWrite(_PWML_pin, pwm);
     }
     
     void pololu_HPMD::reverse(int pwm)
     {
        digitalWrite(_DIR_pin, HIGH);
        digitalWrite(_PWMH_pin, pwm);
        digitalWrite(_PWML_pin, pwm);
     }
     
    float pololu_HPMD::get_current()
    {
        //0 to 1023 analog range
        //204.8 per volt at a 5V refrece
        // 66mv per amp centered at 2.5v
        // 512 is 0 amps
        _cs = analogRead(_cs_pin);
        
        if (_cs == 512) { return (0.0);}
        
        else if (_cs > 512) {return float ((_cs - 512) / 66);}
        
        else { return float (( 512 - _cs ) / 66); }
    }