Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

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?

UserRevisionLine numberNew 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 }