Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
ap1017ctrl.cpp
- Committer:
- masahikofukasawa
- Date:
- 2017-12-13
- Revision:
- 46:5938ad2039b0
- Parent:
- 43:45225713cd58
- Child:
- 47:221ec4b404ec
File content as of revision 46:5938ad2039b0:
#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(P0_11); inputB = new DigitalOut(P0_9); i2cMotor = new I2C(I2C_SDA0, I2C_SCL0); i2cMotor->frequency(I2C_SPEED); switch (subId) { // template for multiple drivers case SUB_ID_AP1017: ap1017 = new AP1017(inputA, inputB, i2cMotor); // 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; } 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(); } }