/*
 * polulu.h
 *
 *  Pololu hardware interface
 *
 *  Created on: 20 Aug 2015
 *      Author: timothyw
 */

#ifndef INCLUDE_NEGOTIATOR_DRIVERS_FLIPPERS_POLOLU_H_
#define INCLUDE_NEGOTIATOR_DRIVERS_FLIPPERS_POLOLU_H_

#include <negotiator_drivers/flippers/hardware.h>
#include <polulu_jrk/polulu_jrk.hpp>

#include <string>

namespace casualty {

class FlippersPololuInterface : public FlippersHardwareInterface {
public:

    FlippersPololuInterface();
    virtual ~FlippersPololuInterface();

    /**
     * Configure the drivers and USB elements
     */
    virtual void configure(crosbot::ConfigElementPtr cf);

    /**
     * Start the driver
     */
    virtual void start();

    /**
     * Stop the driver
     */
    virtual void stop();

    /**
     * Emergency Stop command
     */
    virtual void command_EMStop();

    /**
     * Move at velocity, positive velocity is forward, negative velocity is reverse
     */
    virtual void command_Velocity(double velocity);

    /**
     * Reads a single set of values
     */
    virtual void readValues(double &pot_val, double &setPoint, double &battery);

private:
    // Pololu controller
    PololuArrayJRK* pololu;

    // Need to store configuration since pololu controller starts up with config
    crosbot::ConfigElementPtr config;
};

} // namespace casualty


#endif /* INCLUDE_NEGOTIATOR_DRIVERS_FLIPPERS_POLOLU_H_ */
