jordy morsinkhof / Mbed 2 deprecated Handmatig

Niet van mijzelf

Dependencies:   USBHost USBHostXpad mbed-rtos mbed

Fork of x4180_Tank by C K

Traxster.cpp

Committer:
347467
Date:
2015-02-25
Revision:
10:7e3987c8fa37
Parent:
8:36b2ef26a0b1

File content as of revision 10:7e3987c8fa37:


#include "Traxster.h"
#include "utils.h"
#include "rtos.h"

//extern Serial pc;
//extern Mutex m;

int getMotorSpeedInt(float f){
    
    int max = 40;
    
    if(f > 1.0)
        f = 1.0;

    if(f < -1.0)
        f = -1.0;
    
    return   (int)round( (float)max * f );
}

void Traxster::SetMotors(float fm1, float fm2)
{
    int m1 = getMotorSpeedInt(fm1);
    int m2 = getMotorSpeedInt(fm2);
    
    //clear robot msgs
    while (rob.readable())
        rob.getc();
        
    if (m1 == 0 && m2 == 0)
    {
        rob.puts("stop\r");
    }
    else
    {
        //m.lock();
        //pc.printf("mogo 1:%d 2:%d\r\n", m1, m2);
        //m.unlock();
        rob.printf("mogo 1:%d 2:%d\r", m1, m2);
    }
    
    //clear robot msgs
    while (rob.readable())
        rob.getc();
}