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