elektron.cpp
Go to the documentation of this file.
00001 /*
00002  * elektronv2.cpp
00003  *
00004  *  Created on: May 22, 2012
00005  *      Author: mwalecki
00006  */
00007 
00008 #include <math.h>
00009 #include <unistd.h>
00010 #include <fcntl.h>
00011 #include <sys/types.h>
00012 #include <sys/stat.h>
00013 #include <cstring>
00014 #include <iostream>
00015 
00016 #include "elektronv2.hpp"
00017 
00018 using namespace std;
00019 
00020 NF_STRUCT_ComBuf        NFComBuf;
00021 uint8_t             crcTable[256];
00022 
00023 double ang_nor_rad(double rad) {
00024         static double TWO_PI = 2.0 * M_PI;
00025         for (;;) {
00026                 if (rad >= M_PI)
00027                         rad -= TWO_PI;
00028                 else if (rad <= -M_PI)
00029                         rad += TWO_PI;
00030                 else
00031                         return (rad);
00032         }
00033 }
00034 
00035 Elektron::Elektron(const std::string& port, int baud) {
00036         connected = false;
00037         
00038     crcInit();
00039     
00040     this->txAddr = NF_RobotAddress;
00041     NFComBuf.myAddress = NF_TerminalAddress;
00042     NFComBuf.ReadDeviceVitals.addr[0] = NF_RobotAddress;
00043     NFComBuf.ReadDeviceVitals.addr[1] = NF_RobotAddress;
00044     NFComBuf.ReadDeviceVitals.addr[2] = NF_RobotAddress;
00045     NFComBuf.ReadDeviceVitals.addr[3] = NF_RobotAddress;
00046     NFComBuf.ReadDeviceVitals.addr[4] = NF_RobotAddress;
00047     NFComBuf.ReadDeviceVitals.addr[5] = NF_RobotAddress;
00048     NFComBuf.ReadDeviceVitals.addr[6] = NF_RobotAddress;
00049     NFComBuf.ReadDeviceVitals.addr[7] = NF_RobotAddress;
00050     NFComBuf.SetDrivesMode.addr[0] = NF_RobotAddress;
00051     NFComBuf.SetDrivesMode.addr[1] = NF_RobotAddress;
00052     NFComBuf.SetDrivesSpeed.addr[0] = NF_RobotAddress;
00053     NFComBuf.SetDrivesSpeed.addr[1] = NF_RobotAddress;
00054     NFComBuf.SetDigitalOutputs.addr[0] = NF_RobotAddress;
00055 
00056 
00057         fd = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
00058         if (fd >= 0) {
00059                 tcgetattr(fd, &oldtio);
00060 
00061                 // set up new settings
00062                 struct termios newtio;
00063                 memset(&newtio, 0, sizeof(newtio));
00064                 newtio.c_cflag = CBAUD | CS8 | CLOCAL | CREAD | CSTOPB;
00065                 newtio.c_iflag = INPCK; //IGNPAR;
00066                 newtio.c_oflag = 0;
00067                 newtio.c_lflag = 0;
00068                 if (cfsetispeed(&newtio, baud) < 0 || cfsetospeed(&newtio, baud) < 0) {
00069                         fprintf(stderr, "Failed to set serial baud rate: %d\n", baud);
00070                         tcsetattr(fd, TCSANOW, &oldtio);
00071                         close(fd);
00072                         fd = -1;
00073                         return;
00074                 }
00075                 // activate new settings
00076                 tcflush(fd, TCIFLUSH);
00077                 tcsetattr(fd, TCSANOW, &newtio);
00078                 connected = true;
00079         }
00080 }
00081 
00082 Elektron::~Elektron() {
00083         // restore old port settings
00084         if (fd > 0)
00085                 tcsetattr(fd, TCSANOW, &oldtio);
00086         close(fd);
00087 }
00088 
00089 void Elektron::update() {
00090 
00091         if(commandCnt == 0){
00092                 cout << "No data to send" << endl;
00093                 return;
00094         }
00095         
00096     txCnt = NF_MakeCommandFrame(txBuf, commandArray, commandCnt, txAddr);
00097     clearCommandArray();
00098     
00099         tcflush(fd, TCIFLUSH);
00100         write(fd, txBuf, txCnt);
00101 
00102 //      while (ret < sizeof(getdata))
00103 //              ret += read(fd, ((char*) &getdata) + ret, sizeof(getdata) - ret);
00104 
00105 }
00106 
00107 void Elektron::setVelocity(double lvel, double rvel) {
00108         lvel = (int16_t)(lvel * (1 / m_per_tick) * 0.1); // Convert SI units to internal units
00109         rvel = (int16_t)(rvel * (1 / m_per_tick) * 0.1);
00110 
00111         if (rvel > MAX_VEL)
00112                 rvel = MAX_VEL;
00113         else if (rvel < -MAX_VEL)
00114                 rvel = -MAX_VEL;
00115 
00116         if (lvel > MAX_VEL)
00117                 lvel = MAX_VEL;
00118         else if (lvel < -MAX_VEL)
00119                 lvel = -MAX_VEL;
00120                 
00121         NFComBuf.SetDrivesSpeed.data[0] = lvel;
00122         NFComBuf.SetDrivesSpeed.data[1] = rvel;
00123         
00124         addToCommandArray(NF_COMMAND_SetDrivesSpeed);
00125         
00126         NFComBuf.SetDrivesMode.data[0] = NF_DrivesMode_SPEED;
00127         NFComBuf.SetDrivesMode.data[1] = NF_DrivesMode_SPEED;
00128         
00129         addToCommandArray(NF_COMMAND_SetDrivesMode);
00130         
00131         cout << "set Velocity" << endl;
00132 }
00133 
00134 void Elektron::getVelocity(double &xvel, double &thvel) {
00135 /*      static int maxl = 0, maxr = 0;
00136         double lvel = (double) (getdata.lvel) * m_per_tick * 10;
00137         double rvel = (double) (getdata.rvel) * m_per_tick * 10;
00138 
00139         if (getdata.lvel > maxl) maxl = getdata.lvel;
00140         if (getdata.rvel > maxr) maxr = getdata.rvel;
00141 
00142         //std::cout << maxl << " " << maxr << "\n";
00143         xvel = (lvel + rvel) * 0.5;
00144         thvel = (lvel - rvel) / AXLE_LENGTH;*/
00145 }
00146 
00147 void Elektron::setScale(double ls, double rs) {
00148         lin_scale = ls;
00149         rot_scale = rs;
00150 }
00151 
00152 bool Elektron::isConnected() {
00153         return connected;
00154 }
00155 
00156 void Elektron::addToCommandArray(uint8_t command){
00157         commandArray[commandCnt++] = command;
00158 }
00159 
00160 void Elektron::clearCommandArray(void){
00161         commandCnt = 0;
00162 }
00163 
00164 
00165 
00166 
00167 
00168 
00169 
00170 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


elektron_base
Author(s): Konrad Banachowicz
autogenerated on Thu Nov 14 2013 11:57:03