eBike / Mbed 2 deprecated ENCODER_TEST3_peddep

ti bisogna il phaserunner

Dependencies:   mbed PID mbed-rtos

Peripherien/Phaserunner.cpp@11:39bd79605827, 2019-06-04 (annotated)

Committer:
beacon
Date:
Tue Jun 04 19:03:39 2019 +0000
Revision:
11:39bd79605827
Parent:
9:56aed8c6779f 
ti bisogna il phaserunner

Who changed what in which revision?

UserRevisionLine numberNew contents of line
EpicG10 7:15e6fc689368 1 #include "Phaserunner.h"
EpicG10 7:15e6fc689368 2
beacon 11:39bd79605827 3 Phaserunner::Phaserunner(RawSerial& connection):
beacon 11:39bd79605827 4 connection(connection),
beacon 11:39bd79605827 5 led(LED2),
beacon 11:39bd79605827 6 analogOut(NULL),
beacon 11:39bd79605827 7 PHASERUNNERTYPE(MOTORS){
EpicG10 9:56aed8c6779f 8 this->connection.attach(callback(this, &Phaserunner::Rx_interrupt), Serial::RxIrq);
beacon 11:39bd79605827 9 this->ticker.attach(callback(this, &Phaserunner::reduce_timer), 0.001f);
beacon 11:39bd79605827 10 }
EpicG10 7:15e6fc689368 11
beacon 11:39bd79605827 12 Phaserunner::Phaserunner(RawSerial &connection, AnalogOut *analogOut):
beacon 11:39bd79605827 13 connection(connection),
beacon 11:39bd79605827 14 led(LED2),
beacon 11:39bd79605827 15 analogOut(analogOut),
beacon 11:39bd79605827 16 PHASERUNNERTYPE(PEDALS){
beacon 11:39bd79605827 17 this->connection.attach(callback(this, &Phaserunner::Rx_interrupt), Serial::RxIrq);
beacon 11:39bd79605827 18 this->ticker.attach(callback(this, &Phaserunner::reduce_timer), 0.001f);
EpicG10 7:15e6fc689368 19 }
EpicG10 7:15e6fc689368 20
EpicG10 7:15e6fc689368 21 int Phaserunner::WriteRegister(uint8_t *buf, unsigned short addr, unsigned short value){
EpicG10 7:15e6fc689368 22 // lokale variablen deklarieren
EpicG10 7:15e6fc689368 23 uint16_t crc;
EpicG10 7:15e6fc689368 24
EpicG10 7:15e6fc689368 25 buf[0] = 1; // Slave ID
EpicG10 7:15e6fc689368 26 buf[1] = 0x10; // Function Code
EpicG10 7:15e6fc689368 27
EpicG10 7:15e6fc689368 28 buf[2] = (addr >> 8) & 0xFF; // Start Address High Byte
EpicG10 7:15e6fc689368 29 buf[3] = addr & 0xFF; // Start Address Low Byte
EpicG10 7:15e6fc689368 30
EpicG10 7:15e6fc689368 31 buf[4] = 0; // Number of Registers High Byte
EpicG10 7:15e6fc689368 32 buf[5] = 1; // Number of Registers Low Byte
EpicG10 7:15e6fc689368 33
EpicG10 7:15e6fc689368 34 buf[6] = 2; // Number of Data Bytes
EpicG10 7:15e6fc689368 35
EpicG10 7:15e6fc689368 36 buf[7] = (value >> 8) & 0xFF; // Register Value High Byte
EpicG10 7:15e6fc689368 37 buf[8] = value & 0xFF; // Register Value Low Byte
EpicG10 7:15e6fc689368 38
EpicG10 7:15e6fc689368 39 crc = getCRC(buf, 9); // prüfziffer berechnen
EpicG10 7:15e6fc689368 40
EpicG10 7:15e6fc689368 41 buf[9] = crc & 0xFF; // CRC Low Byte
EpicG10 7:15e6fc689368 42 buf[10] = (crc >> 8) & 0xFF; // CRC High Byte
EpicG10 7:15e6fc689368 43 return 11;
EpicG10 7:15e6fc689368 44 }
EpicG10 7:15e6fc689368 45 int Phaserunner::readRegister(uint8_t *buf, uint16_t registerAddress){
EpicG10 7:15e6fc689368 46 // lokale variablen deklarieren
EpicG10 7:15e6fc689368 47 uint16_t crc;
EpicG10 7:15e6fc689368 48
EpicG10 7:15e6fc689368 49 buf[0] = 1; // Slave ID
EpicG10 7:15e6fc689368 50 buf[1] = 0x03; // Function Code
EpicG10 7:15e6fc689368 51
EpicG10 7:15e6fc689368 52 buf[2] = (registerAddress >> 8) & 0xFF; // Start Address High Byte
EpicG10 7:15e6fc689368 53 buf[3] = registerAddress & 0xFF; // Start Address Low Byte
EpicG10 7:15e6fc689368 54
EpicG10 7:15e6fc689368 55 buf[4] = 0; // Number of Registers High Byte
EpicG10 7:15e6fc689368 56 buf[5] = 4; // Number of Registers Low Byte
EpicG10 7:15e6fc689368 57
EpicG10 7:15e6fc689368 58 crc = getCRC(buf, 6); // prüfziffer berechnen
EpicG10 7:15e6fc689368 59
EpicG10 7:15e6fc689368 60 buf[6] = crc & 0xFF; // CRC Low Byte
EpicG10 7:15e6fc689368 61 buf[7] = (crc >> 8) & 0xFF; // CRC High Byte
EpicG10 7:15e6fc689368 62
EpicG10 7:15e6fc689368 63 return 8;
EpicG10 7:15e6fc689368 64 }
EpicG10 7:15e6fc689368 65 int Phaserunner::sendBuffer(unsigned short adress, unsigned short value){
EpicG10 7:15e6fc689368 66 int length;
EpicG10 7:15e6fc689368 67 length = Phaserunner::WriteRegister(this->writeBuffer, adress, value);
EpicG10 7:15e6fc689368 68
EpicG10 7:15e6fc689368 69 return this->sendBuffer(length);
EpicG10 7:15e6fc689368 70 }
EpicG10 7:15e6fc689368 71 int Phaserunner::sendBuffer(int length){
EpicG10 7:15e6fc689368 72 int i;
EpicG10 7:15e6fc689368 73 for( i=0; i<length; i++ ){
EpicG10 7:15e6fc689368 74 this->connection.putc(this->writeBuffer[i]);
EpicG10 7:15e6fc689368 75 }
EpicG10 7:15e6fc689368 76 return length;
EpicG10 7:15e6fc689368 77 }
EpicG10 7:15e6fc689368 78 int Phaserunner::readBuffer(uint16_t adress){
EpicG10 7:15e6fc689368 79 int length;
EpicG10 7:15e6fc689368 80
EpicG10 7:15e6fc689368 81 length = Phaserunner::readRegister(this->writeBuffer, adress);
EpicG10 7:15e6fc689368 82 length = this->sendBuffer(length);
EpicG10 7:15e6fc689368 83
EpicG10 7:15e6fc689368 84 return 0;
EpicG10 7:15e6fc689368 85 }
EpicG10 7:15e6fc689368 86 uint16_t Phaserunner::getCRC(uint8_t *msgByte, uint8_t length){
EpicG10 7:15e6fc689368 87 uint16_t crcRegister = 0xFFFF;
EpicG10 7:15e6fc689368 88 uint8_t i;
EpicG10 7:15e6fc689368 89 uint8_t j;
EpicG10 7:15e6fc689368 90
EpicG10 7:15e6fc689368 91 for (i = 0; i < length; i++) {
EpicG10 7:15e6fc689368 92 crcRegister ^= msgByte[i];
EpicG10 7:15e6fc689368 93 for (j = 0; j < 8; j++) {
EpicG10 7:15e6fc689368 94 if (crcRegister & 1)
EpicG10 7:15e6fc689368 95 crcRegister = (crcRegister >> 1) ^ 0xA001;
EpicG10 7:15e6fc689368 96 else
EpicG10 7:15e6fc689368 97 crcRegister >>= 1;
EpicG10 7:15e6fc689368 98 } // end for (j)
EpicG10 7:15e6fc689368 99 } // end for (i)
EpicG10 7:15e6fc689368 100
EpicG10 7:15e6fc689368 101 return crcRegister;
EpicG10 7:15e6fc689368 102 }
beacon 11:39bd79605827 103 /*void Phaserunner::writeToPhaserunner(){
EpicG10 7:15e6fc689368 104 //while( true ){
EpicG10 7:15e6fc689368 105 //RPM
EpicG10 7:15e6fc689368 106 this->readRPM();
EpicG10 7:15e6fc689368 107 wait_ms(WRITE_PERIOD);
EpicG10 7:15e6fc689368 108
EpicG10 7:15e6fc689368 109 //Torque
EpicG10 7:15e6fc689368 110 this->writeTorque(this->newTorque);
EpicG10 7:15e6fc689368 111 wait_ms(WRITE_PERIOD);
EpicG10 7:15e6fc689368 112
EpicG10 7:15e6fc689368 113 //Recuperation
EpicG10 7:15e6fc689368 114 this->writeRecuperation(this->newRecuperation);
EpicG10 7:15e6fc689368 115 wait_ms(WRITE_PERIOD);
EpicG10 7:15e6fc689368 116 //}
beacon 11:39bd79605827 117 }*/
EpicG10 7:15e6fc689368 118 void Phaserunner::setTorque(uint8_t torque){
EpicG10 7:15e6fc689368 119 if( torque > 100 ){
EpicG10 7:15e6fc689368 120 torque = 100;
EpicG10 7:15e6fc689368 121 }
EpicG10 7:15e6fc689368 122 if( torque > newTorque && torque - newTorque > MAX_TORQUE_GAIN ){
EpicG10 7:15e6fc689368 123 this->newTorque += MAX_TORQUE_GAIN;
EpicG10 7:15e6fc689368 124 } else {
EpicG10 7:15e6fc689368 125 this->newTorque = torque;
EpicG10 7:15e6fc689368 126 }
beacon 11:39bd79605827 127 switch( PHASERUNNERTYPE ){
beacon 11:39bd79605827 128 case PEDALS: this->analogTorque(newTorque); break;
beacon 11:39bd79605827 129 case MOTORS: this->writeTorque(newTorque); break;
beacon 11:39bd79605827 130 }
EpicG10 7:15e6fc689368 131 }
EpicG10 7:15e6fc689368 132 void Phaserunner::setRecuperation(uint8_t recuperation){
EpicG10 7:15e6fc689368 133 this->newRecuperation = recuperation > 100 ? 100 : recuperation;
EpicG10 7:15e6fc689368 134 }
EpicG10 7:15e6fc689368 135 void Phaserunner::writeTorque(uint8_t torque){
EpicG10 7:15e6fc689368 136 unsigned short value = (torque * 4096) / 100;
beacon 11:39bd79605827 137 wait_ms(timer);
EpicG10 7:15e6fc689368 138 this->sendBuffer(REMOTE_THROTTLE_VOLTAGE, value);
beacon 11:39bd79605827 139 timer = WRITE_PERIOD;
beacon 11:39bd79605827 140 }
beacon 11:39bd79605827 141 void Phaserunner::analogTorque(uint8_t torque){
beacon 11:39bd79605827 142 analogOut->write(torque * 0.01f);
EpicG10 7:15e6fc689368 143 }
EpicG10 7:15e6fc689368 144 void Phaserunner::writeRecuperation(uint8_t recuperation){
EpicG10 7:15e6fc689368 145 unsigned short value = (recuperation * 4096) / 100;
beacon 11:39bd79605827 146 wait_ms(timer);
EpicG10 7:15e6fc689368 147 this->sendBuffer(REMOTE_ANALOG_BREAK_VOLTAGE, value);
beacon 11:39bd79605827 148 timer = WRITE_PERIOD;
EpicG10 7:15e6fc689368 149 }
EpicG10 7:15e6fc689368 150 void Phaserunner::readRPM(){
beacon 11:39bd79605827 151 wait_ms(timer);
EpicG10 7:15e6fc689368 152 this->readBuffer(MOTOR_CURRENT);
beacon 11:39bd79605827 153 timer = WRITE_PERIOD;
EpicG10 7:15e6fc689368 154 }
EpicG10 7:15e6fc689368 155
EpicG10 7:15e6fc689368 156 void Phaserunner::Rx_interrupt(){
EpicG10 7:15e6fc689368 157 enum states {waiting, readAnswer, writeAnswer, error};
EpicG10 7:15e6fc689368 158 uint8_t recByte;
EpicG10 7:15e6fc689368 159 static uint8_t messageLength = 0;
EpicG10 7:15e6fc689368 160 static states state = waiting;
EpicG10 7:15e6fc689368 161 static uint8_t read_buffer_index = 0;
EpicG10 7:15e6fc689368 162
EpicG10 7:15e6fc689368 163 switch( state ){
EpicG10 7:15e6fc689368 164 case waiting:
EpicG10 7:15e6fc689368 165 recByte = this->connection.getc();
EpicG10 7:15e6fc689368 166 read_buffer_index = 0;
EpicG10 7:15e6fc689368 167
EpicG10 7:15e6fc689368 168 switch( recByte ){
EpicG10 7:15e6fc689368 169 case 0x04:
EpicG10 7:15e6fc689368 170 //Read Answer
EpicG10 7:15e6fc689368 171 this->read_buffer[read_buffer_index] = recByte;
EpicG10 7:15e6fc689368 172 state = readAnswer;
EpicG10 7:15e6fc689368 173 break;
EpicG10 7:15e6fc689368 174
EpicG10 7:15e6fc689368 175 case 0x03:
EpicG10 7:15e6fc689368 176 //Write Answer
EpicG10 7:15e6fc689368 177 this->read_buffer[read_buffer_index] = recByte;
EpicG10 7:15e6fc689368 178 state = writeAnswer;
EpicG10 7:15e6fc689368 179 break;
EpicG10 7:15e6fc689368 180
EpicG10 7:15e6fc689368 181 case 0x83: case 0x84:
EpicG10 7:15e6fc689368 182 //Error Read
EpicG10 7:15e6fc689368 183 this->read_buffer[read_buffer_index] = recByte;
EpicG10 7:15e6fc689368 184 state = error;
EpicG10 7:15e6fc689368 185 break;
EpicG10 7:15e6fc689368 186
EpicG10 7:15e6fc689368 187 default: break;
EpicG10 7:15e6fc689368 188 }
EpicG10 7:15e6fc689368 189 break;
EpicG10 7:15e6fc689368 190
EpicG10 7:15e6fc689368 191 case writeAnswer:
EpicG10 7:15e6fc689368 192 recByte = this->connection.getc();
EpicG10 7:15e6fc689368 193 this->read_buffer[read_buffer_index] = recByte;
EpicG10 7:15e6fc689368 194 if( read_buffer_index == 1 ){
EpicG10 7:15e6fc689368 195 messageLength = recByte;
EpicG10 7:15e6fc689368 196 }
EpicG10 7:15e6fc689368 197 else if( read_buffer_index == messageLength + 3 ){
EpicG10 7:15e6fc689368 198 this->current = ((this->read_buffer[2] << 8) | this->read_buffer[3]) / 32.0f;
EpicG10 7:15e6fc689368 199 this->frequency = ((this->read_buffer[4] << 8) | this->read_buffer[5]);
EpicG10 7:15e6fc689368 200 this->voltage = ((this->read_buffer[8] << 8) | this->read_buffer[9]) / 32.0f;
EpicG10 7:15e6fc689368 201 state = waiting;
EpicG10 7:15e6fc689368 202 messageLength = 0;
EpicG10 7:15e6fc689368 203 }
EpicG10 7:15e6fc689368 204 break;
EpicG10 7:15e6fc689368 205
EpicG10 7:15e6fc689368 206 case readAnswer:
EpicG10 7:15e6fc689368 207 recByte = this->connection.getc();
EpicG10 7:15e6fc689368 208 this->read_buffer[read_buffer_index] = recByte;
EpicG10 7:15e6fc689368 209 if( read_buffer_index == 1 ){
EpicG10 7:15e6fc689368 210 messageLength = recByte * 2;
EpicG10 7:15e6fc689368 211 }
EpicG10 7:15e6fc689368 212 else if( read_buffer_index == messageLength + 1 ){
EpicG10 7:15e6fc689368 213 state = waiting;
EpicG10 7:15e6fc689368 214 messageLength = 0;
EpicG10 7:15e6fc689368 215 }
EpicG10 7:15e6fc689368 216 break;
EpicG10 7:15e6fc689368 217 }
EpicG10 7:15e6fc689368 218 read_buffer_index++;
EpicG10 7:15e6fc689368 219
EpicG10 7:15e6fc689368 220 // this->buf[this->bufPointer] = this->connection.getc();
EpicG10 7:15e6fc689368 221 // this->bufPointer++;
EpicG10 7:15e6fc689368 222 //
EpicG10 7:15e6fc689368 223 // if( this->bufPointer >= 13 ){
EpicG10 7:15e6fc689368 224 // this->frequency = ((this->buf[3] << 8) | this->buf[4]);
EpicG10 7:15e6fc689368 225 // this->voltage = ((this->buf[7] << 8) | this->buf[8]) / 60.0f;
EpicG10 7:15e6fc689368 226 // this->current = ((this->buf[9] << 8) | this->buf[10]) / 32.0f;
EpicG10 7:15e6fc689368 227 //
EpicG10 7:15e6fc689368 228 // this->bufPointer = 0;
EpicG10 7:15e6fc689368 229 // }
EpicG10 7:15e6fc689368 230 }
EpicG10 7:15e6fc689368 231
EpicG10 7:15e6fc689368 232 float Phaserunner::getFrequency(){
EpicG10 7:15e6fc689368 233 return this->frequency;
EpicG10 7:15e6fc689368 234 }
EpicG10 7:15e6fc689368 235 float Phaserunner::getCurrent(){
EpicG10 7:15e6fc689368 236 return this->current;
EpicG10 7:15e6fc689368 237 }
EpicG10 7:15e6fc689368 238 float Phaserunner::getVoltage(){
EpicG10 7:15e6fc689368 239 return this->voltage;
EpicG10 7:15e6fc689368 240 }
EpicG10 7:15e6fc689368 241
EpicG10 7:15e6fc689368 242 int Phaserunner::getRegister(int address){
EpicG10 7:15e6fc689368 243
beacon 11:39bd79605827 244 readRegister(this->read_buffer,address);
EpicG10 7:15e6fc689368 245 return this->read_buffer[2]+this->read_buffer[3]<<8;
EpicG10 7:15e6fc689368 246 }
EpicG10 7:15e6fc689368 247
EpicG10 7:15e6fc689368 248 uint16_t Phaserunner::getRecup(){
EpicG10 7:15e6fc689368 249 return this->newRecuperation;
beacon 11:39bd79605827 250 }
beacon 11:39bd79605827 251
beacon 11:39bd79605827 252 void Phaserunner::reduce_timer(){
beacon 11:39bd79605827 253 if( timer ) timer--;
EpicG10 7:15e6fc689368 254 }