/*
 * pololu.cpp
 *
 *  Pololu hardware interface
 *
 *  Created on: 20 Aug 2015
 *      Author: timothyw
 */

#include <ros/ros.h>

#include "negotiator_drivers/flippers/pololu.h"

namespace casualty {

FlippersPololuInterface::FlippersPololuInterface() :
    pololu(NULL),
    config(NULL)
{}

FlippersPololuInterface::~FlippersPololuInterface() {
    if (pololu != NULL) {
        delete pololu;
    }
}

/**
 * Configure the drivers and USB elements
 */
void FlippersPololuInterface::configure(crosbot::ConfigElementPtr cf) {
    // Store config
    config = cf;

    // Create pololu
    pololu = new PololuArrayJRK();
}

/**
 * Start the driver
 */
void FlippersPololuInterface::start() {
    pololu->Initialize(config);
}

/**
 * Stop the driver
 */
void FlippersPololuInterface::stop() {
    pololu->Shutdown();
}

/**
 * Emergency Stop command
 */
void FlippersPololuInterface::command_EMStop() {
    pololu->SetVelocity(0);
}

/**
 * Move at velocity, positive velocity is forward, negative velocity is reverse
 */
void FlippersPololuInterface::command_Velocity(double velocity) {
    pololu->SetVelocity(velocity);
}

/**
 * Reads a single set of values
 */
void FlippersPololuInterface::readValues(double &pot_val, double &setPoint, double &battery) {
    pot_val = pololu->GetPosition();
    setPoint = INFINITY;
    battery = 0;
}

} // namespace casualty
