File content as of revision 49:c8f8946129b6:

#include "ap1017ctrl.h"

Ap1017Ctrl::Ap1017Ctrl()
{    
    ap1017 = NULL;
    sensorName = "";
    interval = 1;
    
    MSG("#AP1017: Created.\r\n");
}

Ap1017Ctrl::~Ap1017Ctrl()
{    
    if(ap1017)
        delete ap1017;
        
    MSG("#AP1017: Destroyed.\r\n");
}

AkmSensor::Status Ap1017Ctrl::init(const uint8_t id, const uint8_t subid)
{    
    primaryId = id;
    subId = subid;
    freq = 100;
    interval = (freq > 0 ? 1/freq : 0);
    
    inputA = new DigitalOut(DIGITAL_D0);
    inputB = new DigitalOut(DIGITAL_D1);
    enable = new DigitalOut(DIGITAL_D9);

    switch (subId) {                // template for multiple drivers
        case SUB_ID_AP1017:
            ap1017 = new AP1017(inputA, inputB, enable);  // Instantiate AP1017
            sensorName = "AP1017";
            MSG("#AP1017 found.\r\n");
            break;
        default:
            MSG("#Error: No sensor found.\r\n.");
            return AkmSensor::ERROR;
    }
    
    // Default settings
    ap1017->setDirection(AP1017::DIRECTION_CW);
    ap1017->setSpeed(50.0);           
    
    MSG("#AP1017: Initialized.\r\n");      

    return AkmSensor::SUCCESS;
}

bool Ap1017Ctrl::isEvent()
{    
    return false;       // No feedback: always false
}

AkmSensor::Status Ap1017Ctrl::startSensor()
{    
    if(freq > 0)
        interval = 1/freq;

    index = (uint8_t)(100.0/(double)(ap1017->getSpeed()));

    MSG("#AP1017: Sensor started with no interval argument.\r\n");
    MSG("#AP1017: Speed = %.2f\r\n", ap1017->getSpeed());
    MSG("#AP1017: Frequency = %.1f Hz\r\n", freq);
    MSG("#AP1017: Index = %d\r\n", index);
    MSG("#AP1017: PWM Period = %.8f\r\n", interval);
    MSG("#AP1017: Pulse Period = %.8f\r\n", interval/index);
    

    pwm.attach(callback(this, &Ap1017Ctrl::pwmPeriod), interval);
    pulse.attach(callback(this, &Ap1017Ctrl::pwmOnPulse), interval/index);
    
    return AkmSensor::SUCCESS;
}

AkmSensor::Status Ap1017Ctrl::startSensor(const float freq)
{
    interval = 1/freq;
    
    MSG("#AP1017: Sensor started with interval argument.\r\n");
    
    return AkmSensor::SUCCESS;
}

AkmSensor::Status Ap1017Ctrl::stopSensor()
{   
    pwm.detach();
    
    ap1017->stop(); 
        
    MSG("#AP1017: Sensor stopped.\r\n");
    
    return AkmSensor::SUCCESS;
}

AkmSensor::Status Ap1017Ctrl::readSensorData(Message* msg)
{
    MSG("#AP1017: No sensor data to read.\r\n");
    
    return AkmSensor::ERROR;
}

AkmSensor::Status Ap1017Ctrl::requestCommand(Message* in, Message* out)
{    
    AkmSensor::Status status = AkmSensor::SUCCESS;
    
    Message::Command cmd = in->getCommand();        // Store command
    out->setCommand(cmd);                           // Load command into output
    
    switch(cmd)
    {
        case Message::CMD_MOTOR_START_MOTOR:
        {
            if(startSensor() != AkmSensor::SUCCESS)
            {
                MSG("#AP1017: Failed to start motor.\r\n");
                return AkmSensor::ERROR;
            }
            else
            {
                MSG("#AP1017: Motor started.\r\n");
            }    
            break;
        }
        case Message::CMD_MOTOR_STOP_MOTOR:
        {
            if(stopSensor() != AkmSensor::SUCCESS)
            {
                MSG("#AP1017: Failed to stop motor.\r\n");
            }
            else
            {
                MSG("#AP1017: Motor stopped.\r\n");
            }    
            break;
        }
        case Message::CMD_MOTOR_SET_DIRECTION:
        {
            if(ap1017->setDirection((AP1017::Rotation)(in->getArgument(0))) != AP1017::SUCCESS)
            {
                MSG("#AP1017: Failed to set motor direction.\r\n");
            }
            else
            {
                MSG("#AP1017: Direction changed.\r\n");
            }    
            break;
        }
        case Message::CMD_MOTOR_SET_DUTY_CYCLE:
        {
            if(ap1017->setSpeed((AP1017::Rotation)(in->getArgument(0))) != AP1017::SUCCESS)
            {
                MSG("#AP1017: Failed to set the duty cycle.\r\n");
            }
            else
            {
                MSG("#AP1017: Duty cycle changed: %.2f.\r\n", ap1017->getSpeed());
            }    
            break;
        }
        default:
        {
            MSG("#AP1017: Invalid or unimplemented command.\r\n");
            status = AkmSensor::ERROR;
        }
    }
        
    return status;
}

const char* Ap1017Ctrl::getSensorName()
{
    MSG("#AP1017: Getting sensor name.\r\n");
    
    return sensorName;
}

// This callback function generates the period for the PWM
void Ap1017Ctrl::pwmPeriod()
{
    if(!ap1017->isMotorOn())
        ap1017->start();   
}

// This callback function generates period for the ON pulse
void Ap1017Ctrl::pwmOnPulse()
{
    static char pulseCounter = 0;
    
    pulseCounter++;
    
    if( (pulseCounter % index == 1) && ap1017->isMotorOn() )
    {
        ap1017->stop();
    }
}