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