Phaserunner.cpp

00001 #include "Phaserunner.h"
00002 
00003 Phaserunner::Phaserunner(RawSerial& connection):
00004 connection(connection),
00005 led(LED2),
00006 analogOut(NULL),
00007 PHASERUNNERTYPE(MOTORS){
00008     this->connection.attach(callback(this, &Phaserunner::Rx_interrupt), Serial::RxIrq);
00009     this->ticker.attach(callback(this, &Phaserunner::reduce_timer), 0.001f);
00010 }
00011 
00012 Phaserunner::Phaserunner(RawSerial &connection, AnalogOut *analogOut):
00013 connection(connection),
00014 led(LED2),
00015 analogOut(analogOut),
00016 PHASERUNNERTYPE(PEDALS){
00017     this->connection.attach(callback(this, &Phaserunner::Rx_interrupt), Serial::RxIrq);
00018     this->ticker.attach(callback(this, &Phaserunner::reduce_timer), 0.001f);
00019 }
00020 
00021 int Phaserunner::WriteRegister(uint8_t *buf, unsigned short addr, unsigned short value){
00022     // lokale variablen deklarieren
00023     uint16_t crc;
00024 
00025     buf[0] = 1;                     // Slave ID
00026     buf[1] = 0x10;                  // Function Code
00027 
00028     buf[2] = (addr >> 8) & 0xFF;    // Start Address High Byte
00029     buf[3] = addr & 0xFF;           // Start Address Low Byte
00030 
00031     buf[4] = 0;                     // Number of Registers High Byte
00032     buf[5] = 1;                     // Number of Registers Low Byte
00033 
00034     buf[6] = 2;                     // Number of Data Bytes
00035 
00036     buf[7] = (value >> 8) & 0xFF;   // Register Value High Byte
00037     buf[8] = value & 0xFF;          // Register Value Low Byte
00038 
00039     crc = getCRC(buf, 9);           // prüfziffer berechnen
00040 
00041     buf[9] = crc & 0xFF;            // CRC Low Byte
00042     buf[10] = (crc >> 8) & 0xFF;    // CRC High Byte
00043     return 11;
00044 }
00045 int Phaserunner::readRegister(uint8_t *buf, uint16_t registerAddress){
00046     // lokale variablen deklarieren
00047     uint16_t crc;
00048 
00049     buf[0] = 1;                             // Slave ID
00050     buf[1] = 0x03;                          // Function Code
00051 
00052     buf[2] = (registerAddress >> 8) & 0xFF; // Start Address High Byte
00053     buf[3] = registerAddress & 0xFF;        // Start Address Low Byte
00054 
00055     buf[4] = 0;                             // Number of Registers High Byte
00056     buf[5] = 4;                             // Number of Registers Low Byte
00057 
00058     crc = getCRC(buf, 6);                   // prüfziffer berechnen
00059 
00060     buf[6] = crc & 0xFF;                    // CRC Low Byte
00061     buf[7] = (crc >> 8) & 0xFF;             // CRC High Byte
00062 
00063     return 8;
00064 }
00065 int Phaserunner::sendBuffer(unsigned short adress, unsigned short value){
00066     int length;
00067     length = Phaserunner::WriteRegister(this->writeBuffer, adress, value);
00068 
00069     return this->sendBuffer(length);
00070 }
00071 int Phaserunner::sendBuffer(int length){
00072     int i;
00073     for( i=0; i<length; i++ ){
00074         this->connection.putc(this->writeBuffer[i]);
00075     }
00076     return length;
00077 }
00078 int Phaserunner::readBuffer(uint16_t adress){
00079     int length;
00080 
00081     length = Phaserunner::readRegister(this->writeBuffer, adress);
00082     length = this->sendBuffer(length);
00083 
00084     return 0;
00085 }
00086 uint16_t Phaserunner::getCRC(uint8_t *msgByte, uint8_t length){
00087     uint16_t crcRegister = 0xFFFF;
00088     uint8_t i;
00089     uint8_t j;
00090 
00091     for (i = 0; i < length; i++) {
00092         crcRegister ^= msgByte[i];
00093         for (j = 0; j < 8; j++) {
00094             if (crcRegister & 1)
00095                 crcRegister = (crcRegister >> 1) ^ 0xA001;
00096             else
00097                 crcRegister >>= 1;
00098         } // end for (j)
00099     } // end for (i)
00100 
00101     return crcRegister;
00102 }
00103 /*void Phaserunner::writeToPhaserunner(){
00104     //while( true ){
00105     //RPM
00106     this->readRPM();
00107     wait_ms(WRITE_PERIOD);
00108 
00109     //Torque
00110     this->writeTorque(this->newTorque);
00111     wait_ms(WRITE_PERIOD);
00112 
00113     //Recuperation
00114     this->writeRecuperation(this->newRecuperation);
00115     wait_ms(WRITE_PERIOD);
00116     //}
00117 }*/
00118 void Phaserunner::setTorque(uint8_t torque){
00119     if( torque > 100 ){
00120         torque = 100;
00121     }
00122     if( torque > newTorque && torque - newTorque > MAX_TORQUE_GAIN ){
00123         this->newTorque += MAX_TORQUE_GAIN;
00124     } else {
00125         this->newTorque = torque;
00126     }
00127     switch( PHASERUNNERTYPE ){
00128         case PEDALS: this->analogTorque(newTorque); break;
00129         case MOTORS: this->writeTorque(newTorque);  break;
00130     }
00131 }
00132 void Phaserunner::setRecuperation(uint8_t recuperation){
00133     this->newRecuperation = recuperation > 100 ? 100 : recuperation;
00134 }
00135 void Phaserunner::writeTorque(uint8_t torque){
00136     unsigned short value = (torque * 4096) / 100;
00137     wait_ms(timer);
00138     this->sendBuffer(REMOTE_THROTTLE_VOLTAGE, value);
00139     timer = WRITE_PERIOD;
00140 }
00141 void Phaserunner::analogTorque(uint8_t torque){
00142     analogOut->write(torque * 0.01f);
00143 }
00144 void Phaserunner::writeRecuperation(uint8_t recuperation){
00145     unsigned short value = (recuperation * 4096) / 100;
00146     wait_ms(timer);
00147     this->sendBuffer(REMOTE_ANALOG_BREAK_VOLTAGE, value);
00148     timer = WRITE_PERIOD;
00149 }
00150 void Phaserunner::readRPM(){
00151     wait_ms(timer);
00152     this->readBuffer(MOTOR_CURRENT);
00153     timer = WRITE_PERIOD;
00154 }
00155 
00156 void Phaserunner::Rx_interrupt(){
00157     enum states {waiting, readAnswer, writeAnswer, error};
00158     uint8_t recByte;
00159     static uint8_t messageLength = 0;
00160     static states state = waiting;
00161     static uint8_t read_buffer_index = 0;
00162 
00163     switch( state ){
00164         case waiting:
00165             recByte = this->connection.getc();
00166             read_buffer_index = 0;
00167 
00168             switch( recByte ){
00169                 case 0x04:
00170                     //Read Answer
00171                     this->read_buffer[read_buffer_index] = recByte;
00172                     state = readAnswer;
00173                     break;
00174 
00175                 case 0x03:
00176                     //Write Answer
00177                     this->read_buffer[read_buffer_index] = recByte;
00178                     state = writeAnswer;
00179                     break;
00180 
00181                 case 0x83: case 0x84:
00182                     //Error Read
00183                     this->read_buffer[read_buffer_index] = recByte;
00184                     state = error;
00185                     break;
00186 
00187                 default: break;
00188             }
00189             break;
00190 
00191         case writeAnswer:
00192             recByte = this->connection.getc();
00193             this->read_buffer[read_buffer_index] = recByte;
00194             if( read_buffer_index == 1 ){
00195                 messageLength = recByte;
00196             }
00197             else if( read_buffer_index == messageLength + 3 ){
00198                 this->current =     ((this->read_buffer[2] << 8) | this->read_buffer[3]) / 32.0f;
00199                 this->frequency =   ((this->read_buffer[4] << 8) | this->read_buffer[5]);
00200                 this->voltage =     ((this->read_buffer[8] << 8) | this->read_buffer[9]) / 32.0f;
00201                 state = waiting;
00202                 messageLength = 0;
00203             }
00204             break;
00205 
00206         case readAnswer:
00207             recByte = this->connection.getc();
00208             this->read_buffer[read_buffer_index] = recByte;
00209             if( read_buffer_index == 1 ){
00210                 messageLength = recByte * 2;
00211             }
00212             else if( read_buffer_index == messageLength + 1 ){
00213                 state = waiting;
00214                 messageLength = 0;
00215             }
00216             break;
00217     }
00218     read_buffer_index++;
00219 
00220 //    this->buf[this->bufPointer] = this->connection.getc();
00221 //    this->bufPointer++;
00222 //
00223 //    if( this->bufPointer >= 13 ){
00224 //        this->frequency =   ((this->buf[3] << 8) | this->buf[4]);
00225 //        this->voltage =     ((this->buf[7] << 8) | this->buf[8]) / 60.0f;
00226 //        this->current =     ((this->buf[9] << 8) | this->buf[10]) / 32.0f;
00227 //
00228 //        this->bufPointer = 0;
00229 //    }
00230 }
00231 
00232 float Phaserunner::getFrequency(){
00233     return this->frequency;
00234 }
00235 float Phaserunner::getCurrent(){
00236     return this->current;
00237 }
00238 float Phaserunner::getVoltage(){
00239     return this->voltage;
00240 }
00241 
00242 int Phaserunner::getRegister(int address){
00243 
00244     readRegister(this->read_buffer,address);
00245     return this->read_buffer[2]+this->read_buffer[3]<<8;
00246 }
00247 
00248 uint16_t Phaserunner::getRecup(){
00249     return this->newRecuperation;
00250 }
00251 
00252 void Phaserunner::reduce_timer(){
00253     if( timer ) timer--;
00254 }