ikarashiMDC.cpp

00001 #include "mbed.h"
00002 #include "ikarashiMDC.h"
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 ikarashiMDC::ikarashiMDC(uint8_t taddr,uint8_t tmotorNum,bool tmode,CAN *can)
00015 {
00016     can_driver = can;
00017     addr = taddr;
00018     motorNum = tmotorNum;
00019     mode = tmode;
00020     braking = true;
00021 }
00022 int ikarashiMDC::setSpeed(const double& speed)
00023 {
00024     uint8_t data[4],dataSpeed;
00025     //Limiter
00026     const double cropped_speed = std::min(1.0,std::max(-1.0,speed));
00027     //dataspeed 0~253 neutaral 126
00028     dataSpeed = ((cropped_speed+1.0)/2.0)*253;
00029     //printf("%d\n",dataSpeed);
00030     //set sending data
00031     //msg.data[0] = 255; //header
00032     msg.data[0] = (addr<<5) + motorNum + (mode<<4)+(braking<<3); //address
00033     msg.data[1] = dataSpeed;
00034     msg.data[2] = data[1]^data[2];
00035     msg.id = 1;
00036     msg.len=3;
00037     can_driver->write(msg);
00038     //send data
00039     //for(int i=0; i<4; i++) {
00040         //serial->putc(data[i]);
00041     //}
00042     return 0;
00043 }
00044 
00045 
00046 void estop(Serial *serial)
00047 {
00048     for(int i = 0; i<3; i++) {
00049         serial->putc(255);
00050     }
00051 
00052 }