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