Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

ap1017ctrl.cpp@47:221ec4b404ec, 2017-12-14 (annotated)

Committer:
masahikofukasawa
Date:
Thu Dec 14 00:21:43 2017 +0000
Revision:
47:221ec4b404ec
Parent:
46:5938ad2039b0
Child:
48:427bdb7bf31b
debugged for warning. again.

Who changed what in which revision?

UserRevisionLine numberNew 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
masahikofukasawa 47:221ec4b404ec 173 const 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 }