Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

ap1017ctrl.cpp@36:aafd15b3291b, 2017-05-19 (annotated)

Committer:
tkstreet
Date:
Fri May 19 23:13:23 2017 +0000
Revision:
36:aafd15b3291b
Parent:
33:d3e1e9eb2ef9
Child:
40:42e48427e4b7
Added DigitalOut and I2C definitions to AP1017 control class.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tkstreet 25:76c11ab5060e 1 #include "ap1017ctrl.h"
tkstreet 25:76c11ab5060e 2 #include "debug.h"
tkstreet 25:76c11ab5060e 3
tkstreet 36:aafd15b3291b 4 #define I2C_SPEED 400000
tkstreet 36:aafd15b3291b 5
tkstreet 25:76c11ab5060e 6 Ap1017Ctrl::Ap1017Ctrl()
tkstreet 36:aafd15b3291b 7 {
tkstreet 25:76c11ab5060e 8 ap1017 = NULL;
tkstreet 25:76c11ab5060e 9 sensorName = "";
tkstreet 36:aafd15b3291b 10 interval = 1;
tkstreet 33:d3e1e9eb2ef9 11
tkstreet 33:d3e1e9eb2ef9 12 MSG("#AP1017: Created.\r\n");
tkstreet 25:76c11ab5060e 13 }
tkstreet 25:76c11ab5060e 14
tkstreet 25:76c11ab5060e 15 Ap1017Ctrl::~Ap1017Ctrl()
tkstreet 36:aafd15b3291b 16 {
tkstreet 25:76c11ab5060e 17 if(ap1017)
tkstreet 25:76c11ab5060e 18 delete ap1017;
tkstreet 36:aafd15b3291b 19
tkstreet 36:aafd15b3291b 20 MSG("#AP1017: Destroyed.\r\n");
tkstreet 25:76c11ab5060e 21 }
tkstreet 25:76c11ab5060e 22
tkstreet 25:76c11ab5060e 23 AkmSensor::Status Ap1017Ctrl::init(const uint8_t id, const uint8_t subid)
tkstreet 36:aafd15b3291b 24 {
tkstreet 25:76c11ab5060e 25 primaryId = id;
tkstreet 25:76c11ab5060e 26 subId = subid;
tkstreet 36:aafd15b3291b 27 freq = 100;
tkstreet 36:aafd15b3291b 28 interval = (freq > 0 ? 1/freq : 0);
tkstreet 36:aafd15b3291b 29
tkstreet 36:aafd15b3291b 30 inputA = new DigitalOut(P0_11);
tkstreet 36:aafd15b3291b 31 inputB = new DigitalOut(P0_9);
tkstreet 36:aafd15b3291b 32 i2cMotor = new I2C(I2C_SDA0, I2C_SCL0);
tkstreet 36:aafd15b3291b 33 i2cMotor->frequency(I2C_SPEED);
tkstreet 25:76c11ab5060e 34
tkstreet 26:4e436b0cbaf8 35 switch (subId) { // template for multiple drivers
tkstreet 25:76c11ab5060e 36 case SUB_ID_AP1017:
tkstreet 36:aafd15b3291b 37 ap1017 = new AP1017(inputA, inputB, i2cMotor); // Instantiate AP1017
tkstreet 25:76c11ab5060e 38 sensorName = "AP1017";
tkstreet 33:d3e1e9eb2ef9 39 MSG("#AP1017 found.\r\n");
tkstreet 25:76c11ab5060e 40 break;
tkstreet 25:76c11ab5060e 41 default:
tkstreet 33:d3e1e9eb2ef9 42 MSG("#Error: No sensor found.\r\n.");
tkstreet 25:76c11ab5060e 43 return AkmSensor::ERROR;
tkstreet 25:76c11ab5060e 44 }
tkstreet 25:76c11ab5060e 45
tkstreet 32:42c6b8fb1922 46 // Default settings
tkstreet 32:42c6b8fb1922 47 ap1017->setDirection(AP1017::DIRECTION_CW);
tkstreet 36:aafd15b3291b 48 ap1017->setSpeed(50.0);
tkstreet 36:aafd15b3291b 49
tkstreet 36:aafd15b3291b 50 MSG("#AP1017: Initialized.\r\n");
tkstreet 25:76c11ab5060e 51
tkstreet 25:76c11ab5060e 52 return AkmSensor::SUCCESS;
tkstreet 25:76c11ab5060e 53 }
tkstreet 25:76c11ab5060e 54
tkstreet 25:76c11ab5060e 55 bool Ap1017Ctrl::isEvent()
tkstreet 33:d3e1e9eb2ef9 56 {
tkstreet 36:aafd15b3291b 57 return false; // No feedback: always false
tkstreet 25:76c11ab5060e 58 }
tkstreet 25:76c11ab5060e 59
tkstreet 25:76c11ab5060e 60 AkmSensor::Status Ap1017Ctrl::startSensor()
tkstreet 36:aafd15b3291b 61 {
tkstreet 36:aafd15b3291b 62 if(freq > 0)
tkstreet 36:aafd15b3291b 63 interval = 1/freq;
tkstreet 36:aafd15b3291b 64
tkstreet 36:aafd15b3291b 65 index = (uint8_t)(100.0/(ap1017->getSpeed()));
tkstreet 36:aafd15b3291b 66
tkstreet 36:aafd15b3291b 67 MSG("#AP1017: Sensor started with no interval argument.\r\n");
tkstreet 36:aafd15b3291b 68 MSG("#AP1017: Speed = %.2f\r\n", ap1017->getSpeed());
tkstreet 36:aafd15b3291b 69 MSG("#AP1017: Frequency = %.1f Hz\r\n", freq);
tkstreet 36:aafd15b3291b 70 MSG("#AP1017: Index = %d\r\n", index);
tkstreet 36:aafd15b3291b 71 MSG("#AP1017: PWM Period = %.8f\r\n", interval);
tkstreet 36:aafd15b3291b 72 MSG("#AP1017: Pulse Period = %.8f\r\n", interval/index);
tkstreet 33:d3e1e9eb2ef9 73
tkstreet 36:aafd15b3291b 74
tkstreet 36:aafd15b3291b 75 pwm.attach(callback(this, &Ap1017Ctrl::pwmPeriod), interval);
tkstreet 36:aafd15b3291b 76 pulse.attach(callback(this, &Ap1017Ctrl::pwmOnPulse), interval/index);
tkstreet 36:aafd15b3291b 77
tkstreet 25:76c11ab5060e 78 return AkmSensor::SUCCESS;
tkstreet 25:76c11ab5060e 79 }
tkstreet 25:76c11ab5060e 80
tkstreet 36:aafd15b3291b 81 AkmSensor::Status Ap1017Ctrl::startSensor(const float freq)
tkstreet 25:76c11ab5060e 82 {
tkstreet 36:aafd15b3291b 83 interval = 1/freq;
tkstreet 33:d3e1e9eb2ef9 84
tkstreet 36:aafd15b3291b 85 MSG("#AP1017: Sensor started with interval argument.\r\n");
tkstreet 32:42c6b8fb1922 86
tkstreet 25:76c11ab5060e 87 return AkmSensor::SUCCESS;
tkstreet 25:76c11ab5060e 88 }
tkstreet 25:76c11ab5060e 89
tkstreet 25:76c11ab5060e 90 AkmSensor::Status Ap1017Ctrl::stopSensor()
tkstreet 36:aafd15b3291b 91 {
tkstreet 36:aafd15b3291b 92 pwm.detach();
tkstreet 33:d3e1e9eb2ef9 93
tkstreet 36:aafd15b3291b 94 ap1017->stop();
tkstreet 36:aafd15b3291b 95
tkstreet 36:aafd15b3291b 96 MSG("#AP1017: Sensor stopped.\r\n");
tkstreet 32:42c6b8fb1922 97
tkstreet 25:76c11ab5060e 98 return AkmSensor::SUCCESS;
tkstreet 25:76c11ab5060e 99 }
tkstreet 25:76c11ab5060e 100
tkstreet 25:76c11ab5060e 101 AkmSensor::Status Ap1017Ctrl::readSensorData(Message* msg)
tkstreet 25:76c11ab5060e 102 {
tkstreet 33:d3e1e9eb2ef9 103 MSG("#AP1017: No sensor data to read.\r\n");
tkstreet 33:d3e1e9eb2ef9 104
tkstreet 33:d3e1e9eb2ef9 105 return AkmSensor::ERROR;
tkstreet 25:76c11ab5060e 106 }
tkstreet 25:76c11ab5060e 107
tkstreet 25:76c11ab5060e 108 AkmSensor::Status Ap1017Ctrl::requestCommand(Message* in, Message* out)
tkstreet 36:aafd15b3291b 109 {
tkstreet 31:8635be9b2e35 110 AkmSensor::Status status = AkmSensor::SUCCESS;
tkstreet 31:8635be9b2e35 111
tkstreet 31:8635be9b2e35 112 Message::Command cmd = in->getCommand(); // Store command
tkstreet 31:8635be9b2e35 113 out->setCommand(cmd); // Load command into output
tkstreet 31:8635be9b2e35 114
tkstreet 31:8635be9b2e35 115 switch(cmd)
tkstreet 31:8635be9b2e35 116 {
tkstreet 31:8635be9b2e35 117 case Message::CMD_MOTOR_START_MOTOR:
tkstreet 31:8635be9b2e35 118 {
tkstreet 36:aafd15b3291b 119 if(startSensor() != AkmSensor::SUCCESS)
tkstreet 31:8635be9b2e35 120 {
tkstreet 31:8635be9b2e35 121 MSG("#AP1017: Failed to start motor.\r\n");
tkstreet 31:8635be9b2e35 122 return AkmSensor::ERROR;
tkstreet 31:8635be9b2e35 123 }
tkstreet 33:d3e1e9eb2ef9 124 else
tkstreet 33:d3e1e9eb2ef9 125 {
tkstreet 33:d3e1e9eb2ef9 126 MSG("#AP1017: Motor started.\r\n");
tkstreet 33:d3e1e9eb2ef9 127 }
tkstreet 31:8635be9b2e35 128 break;
tkstreet 31:8635be9b2e35 129 }
tkstreet 31:8635be9b2e35 130 case Message::CMD_MOTOR_STOP_MOTOR:
tkstreet 31:8635be9b2e35 131 {
tkstreet 36:aafd15b3291b 132 if(stopSensor() != AkmSensor::SUCCESS)
tkstreet 31:8635be9b2e35 133 {
tkstreet 31:8635be9b2e35 134 MSG("#AP1017: Failed to stop motor.\r\n");
tkstreet 31:8635be9b2e35 135 }
tkstreet 33:d3e1e9eb2ef9 136 else
tkstreet 33:d3e1e9eb2ef9 137 {
tkstreet 33:d3e1e9eb2ef9 138 MSG("#AP1017: Motor stopped.\r\n");
tkstreet 33:d3e1e9eb2ef9 139 }
tkstreet 31:8635be9b2e35 140 break;
tkstreet 31:8635be9b2e35 141 }
tkstreet 31:8635be9b2e35 142 case Message::CMD_MOTOR_SET_DIRECTION:
tkstreet 31:8635be9b2e35 143 {
tkstreet 32:42c6b8fb1922 144 if(ap1017->setDirection((AP1017::Rotation)(in->getArgument(0))) != AP1017::SUCCESS)
tkstreet 31:8635be9b2e35 145 {
tkstreet 31:8635be9b2e35 146 MSG("#AP1017: Failed to set motor direction.\r\n");
tkstreet 31:8635be9b2e35 147 }
tkstreet 33:d3e1e9eb2ef9 148 else
tkstreet 33:d3e1e9eb2ef9 149 {
tkstreet 33:d3e1e9eb2ef9 150 MSG("#AP1017: Direction changed.\r\n");
tkstreet 33:d3e1e9eb2ef9 151 }
tkstreet 31:8635be9b2e35 152 break;
tkstreet 31:8635be9b2e35 153 }
tkstreet 31:8635be9b2e35 154 case Message::CMD_MOTOR_SET_DUTY_CYCLE:
tkstreet 31:8635be9b2e35 155 {
tkstreet 32:42c6b8fb1922 156 if(ap1017->setSpeed((AP1017::Rotation)(in->getArgument(0))) != AP1017::SUCCESS)
tkstreet 31:8635be9b2e35 157 {
tkstreet 31:8635be9b2e35 158 MSG("#AP1017: Failed to set the duty cycle.\r\n");
tkstreet 31:8635be9b2e35 159 }
tkstreet 33:d3e1e9eb2ef9 160 else
tkstreet 33:d3e1e9eb2ef9 161 {
tkstreet 36:aafd15b3291b 162 MSG("#AP1017: Duty cycle changed: %.2f.\r\n", ap1017->getSpeed());
tkstreet 33:d3e1e9eb2ef9 163 }
tkstreet 31:8635be9b2e35 164 break;
tkstreet 31:8635be9b2e35 165 }
tkstreet 31:8635be9b2e35 166 default:
tkstreet 31:8635be9b2e35 167 {
tkstreet 33:d3e1e9eb2ef9 168 MSG("#AP1017: Invalid or unimplemented command.\r\n");
tkstreet 31:8635be9b2e35 169 status = AkmSensor::ERROR;
tkstreet 31:8635be9b2e35 170 }
tkstreet 31:8635be9b2e35 171 }
tkstreet 31:8635be9b2e35 172
tkstreet 31:8635be9b2e35 173 return status;
tkstreet 25:76c11ab5060e 174 }
tkstreet 25:76c11ab5060e 175
tkstreet 25:76c11ab5060e 176 char* Ap1017Ctrl::getSensorName()
tkstreet 25:76c11ab5060e 177 {
tkstreet 33:d3e1e9eb2ef9 178 MSG("#AP1017: Getting sensor name.\r\n");
tkstreet 33:d3e1e9eb2ef9 179
tkstreet 26:4e436b0cbaf8 180 return sensorName;
tkstreet 31:8635be9b2e35 181 }
tkstreet 31:8635be9b2e35 182
tkstreet 36:aafd15b3291b 183 // This callback function generates the period for the PWM
tkstreet 36:aafd15b3291b 184 void Ap1017Ctrl::pwmPeriod()
tkstreet 31:8635be9b2e35 185 {
tkstreet 36:aafd15b3291b 186 if(!ap1017->isMotorOn())
tkstreet 36:aafd15b3291b 187 ap1017->start();
tkstreet 36:aafd15b3291b 188 }
tkstreet 36:aafd15b3291b 189
tkstreet 36:aafd15b3291b 190 // This callback function generates period for the ON pulse
tkstreet 36:aafd15b3291b 191 void Ap1017Ctrl::pwmOnPulse()
tkstreet 36:aafd15b3291b 192 {
tkstreet 36:aafd15b3291b 193 static char pulseCounter = 0;
tkstreet 33:d3e1e9eb2ef9 194
tkstreet 36:aafd15b3291b 195 pulseCounter++;
tkstreet 36:aafd15b3291b 196
tkstreet 36:aafd15b3291b 197 if( (pulseCounter % index == 1) && ap1017->isMotorOn() )
tkstreet 36:aafd15b3291b 198 {
tkstreet 36:aafd15b3291b 199 ap1017->stop();
tkstreet 36:aafd15b3291b 200 }
tkstreet 25:76c11ab5060e 201 }