ikarashiMDC.cpp

00001 #include "ikarashiMDC.h"
00002 
00003 
00004 ikarashiMDC::ikarashiMDC(DigitalOut* serialcontrol,uint8_t taddr,uint8_t tmotorNum,bool tmode,Serial *tserial)
00005 {
00006     serialControl = serialcontrol;
00007     serialControl->write(true);
00008     serial = tserial;
00009     addr = taddr;
00010     motorNum = tmotorNum;
00011     mode = tmode;
00012     braking = true;
00013 }
00014 
00015 int ikarashiMDC::setSpeed(const double& speed)
00016 {
00017     uint8_t data[4],dataSpeed;
00018     //Limiter
00019     const double cropped_speed = std::min(1.0,std::max(-1.0,speed));
00020     //dataspeed 0~253 neutaral 126
00021     dataSpeed = ((cropped_speed+1.0)/2.0)*253;
00022     //printf("%d\n",dataSpeed);
00023     //set sending data
00024     data[0] = 255; //header
00025     data[1] = (addr<<5) + motorNum + (mode<<4)+(braking<<3); //address
00026     data[2] = dataSpeed;
00027     data[3] = data[1]^data[2];
00028     //send data
00029     for(int i=0; i<4; i++) {
00030         serial->putc(data[i]);
00031     }
00032     return 0;
00033 }
00034 
00035 
00036 void estop(Serial *serial)
00037 {
00038     for(int i = 0; i<3; i++) {
00039         serial->putc(255);
00040     }
00041 
00042 }