eBike / Mbed 2 deprecated ENCODER_TEST3_peddep

ti bisogna il phaserunner

Dependencies:   mbed PID mbed-rtos

Peripherien/Phaserunner.cpp@7:15e6fc689368, 2019-05-16 (annotated)

Committer:
EpicG10
Date:
Thu May 16 20:42:39 2019 +0000
Revision:
7:15e6fc689368
Child:
9:56aed8c6779f
Implementation Regler (not finished)

Who changed what in which revision?

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