NagaokaRoboticsClub_mbedTeam / ikarashiMDC

MDC3.0 rs-485 コントロールプログラム can も追加予定.... 詳細はwiki バグレポートは中田くんに,やさしく教えてください(煽り禁止)

Dependents:   quadOmni_yanagi NHK2017_octopus ikarashiMDCmaster NHK2017_octopus2 ... more

ikarashiMDC.cpp

Committer:
WAT34
Date:
2017-08-10
Revision:
1:5e8014a1adbd
Parent:
0:314f2bed3958
Child:
2:75153205d6e8

File content as of revision 1:5e8014a1adbd:

#include "ikarashiMDC.h"


ikarashiMDC::ikarashiMDC()
{
    serialControl = new DigitalOut(D2);
    serial=NULL;
    addr = 0;
    motorNum = 0;
    serialControl->write(true);
}

int ikarashiMDC::setMotorinfo(uint8_t _addr,uint8_t _motorNum,Serial *_serial)
{
    serial = _serial;
    addr = _addr;
    motorNum = _motorNum;
    return 0;
}

int ikarashiMDC::setMotorSpeed(double speed)
{
    uint8_t data[3],dataSpeed;
    //Limiter
    if(speed > 1.0) speed = 1.0;
    if(speed < -1.0) speed = -1.0;
    
    //dataspeed 0~253 neutaral 126 
    dataSpeed = ((speed+1.0)/2.0)*253;
    //set sending data
    data[0] = 255; //header
    data[1] = addr<<4 + motorNum; //address
    data[2] = dataSpeed;
    
    //send data
    for(int i=0;i<3; i++)
    {
        serial->putc(data[i]);
    }
    return 0;
 }
void ikarashiMDC::stop() 
{
    int data[3];
    data[0] = 255;
    data[2] = 126;
    for(int i = 0; i < 16;i++)
    {
        for(int j = 0;j <3;j++)
        {
            for(int k = 0;j<3;j++)
            {
                data[1] = i<<4 + j;
                serial->putc(data[k]);
            }
        }
    }
     
}
int ikarashiMDC::checkcomm()
{
    int data[3];
    data[0] =255;
    data[1] = addr<<4 +motorNum;
    data[2]=126;
    for(int i=0;i<3;i++)
        serial->putc(data[i]);
    serialControl->write(false);
    if (serial->readable())
        return serial->getc();
    serialControl->write(true);
    return false;
}