File content as of revision 0:5e55308aa7a6:

#include "mbed.h"
#include "madpulse.h"
#define DT 0.02
MadPulse::MadPulse(uint8_t id,Serial *port,BNO055 *imu, ServoIn *CH1,ServoIn *CH2, InterruptIn *irq, ServoOut *Steer, ServoOut *Throttle):
    id(id),
    _imu(imu),
    _port(port),
    _wheel_irq(irq),
    _CH1(CH1),
    _CH2(CH2),
    _Steer(Steer),
    _Throttle(Throttle)
    {
    ti.start(); //Start Local Timer

    _wheel_irq->rise(this, &MadPulse::wheelCallback); // Callback to Interrupt for Wheel Sensor

    if(_imu->check()) {

        printf("BNO055 connected\r\n");
        _imu->setmode(OPERATION_MODE_CONFIG);
        _imu->SetExternalCrystal(1);
        _imu->setmode(OPERATION_MODE_NDOF);  //Uses magnetometer
        _imu->set_angle_units(RADIANS);
        _imu->set_accel_units(MPERSPERS);
        _imu->set_anglerate_units(RAD_PER_SEC);          
        _imu->setoutputformat(WINDOWS);
        _imu->set_mapping(2);

  //      imu_LED = 1; // turns on IMU light when calibration is successful

    }
   // _port->attach(this,&MadPulse::rxCallback);

};
void MadPulse::sampleData(){
getAccel();
getGyro();
getMag();
getEuler();
getSpeed();

//printf("Wheel Freq: %f\n\r",wheel_freq);

}
void MadPulse::getAccel(){
    _imu->get_accel();
    accel.x = _imu->accel.x;
    accel.y = _imu->accel.y;
    accel.z = _imu->accel.z;
}
void MadPulse::getGyro(){
    _imu->get_gyro();
    gyro.x = _imu->gyro.x;
    gyro.y = _imu->gyro.y;
    gyro.z = _imu->gyro.z;    
}
void MadPulse::getMag(){
    _imu->get_mag();
    mag.x = _imu->mag.x;
    mag.y = _imu->mag.y;
    mag.z = _imu->mag.z; 
   // printf("Mag:%.1f,%.1f,%.1f\n\r",_imu->mag.x,_imu->mag.y,_imu->mag.z);         
}
void MadPulse::getEuler(){
    _imu->get_angles();
    euler.roll = _imu->euler.roll;
    euler.pitch = _imu->euler.pitch;
    euler.yaw = _imu->euler.yaw;      
}


void MadPulse::getSpeed(){

float dt_loc = ti.read()-t_ws;

    
    if(dt_loc > 0.1){
        wheel_freq = 0;
    }
  /*  if( (dt < 0.05) && (dt > 0.002)) {
      //  wheel_spd = (1/CTS_REV)/(dt_ws); // 0.036 is the wheel radius  v = omega*r
        //wheel_freq = 1/dt_ws;
    } else {
       

    }*/
    
    

}

void MadPulse::resetEnc(){
    counts = 0;
}
void MadPulse::wheelCallback()
{
    dt = ti.read()-t_ws;
    
  //  if(dt < 0.0001){
    if(dt < 0.0001){    
        wheel_freq = 1/dt_old;
        dt_old = dt_old;

    }else{
        wheel_freq = 1/dt;
         dt_old = dt;
    }
    
    if(wheel_freq >200){
        wheel_freq=wheel_freq_old;
    } 
    wheel_freq_old = wheel_freq;
  /*  if(dt > 0.1){
        wheel_freq = 0;
    }*/
   
  
    // Hall effect speed sensor callback
    wheel_LED = !wheel_LED;
    counts++; // Iterate tick counter
    dt_ws = ti.read()-t_ws;
    t_ws= ti.read();
} // end void he_callback

void MadPulse::setSteer(float str){
str = saturateCmd(str);

_Steer->write((int)((str*500.0)+1500.0));  // Write Steering Pulse
}
void MadPulse::setThrottle(float thr){
thr = saturateVal(thr,0.3,-0.3);
_Throttle->write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
}


uint8_t MadPulse::sendMessage(Serial *obj,char* msg,uint8_t len)
{
    int i;
    for(i=0; i<len; i++) {
        obj->printf("%c",msg[i]);
    }
    obj->printf("\r\n");
    return i;
}

void MadPulse::rxCallback(){

                char c = _port->getc();
               // pc.printf("%s",c);
               if(c == 'H' || c == 'h') {
                        _port->printf("Command List...\r\n");
                        _port->printf("d - set open loop duty cycle\r\n");
                        _port->printf("r - run open loop control with current duty cycle\r\n");
                        _port->printf("h - display this prompt and exit\r\n");
                        _port->printf("t - set duration of test\r\n\n\n");
                        _port->printf("Enter Command\r\n");
                }
               else if(c == 'R' || c == 'r') {
                   // auto_flag = 1;
                    auto_LED = 1;
                }    
               else if(c == 'S' || c == 's') {
                  //  auto_flag = 0;
                    auto_LED = 0;
                }
                else {
                
                }                            
}


int MadPulse::parseMessage(char * msg)
{
    
    if(!strncmp(msg, "$POS", 4)) {
      //  int arg1;
        float arg2,arg3,arg4;

        sscanf(msg,"$POS,%f,%f,%f\n",&arg2,&arg3,&arg4);

            pos.x = arg2;
            pos.y = arg3;
            pos.z = arg4;

     //  pc.printf("idm: %d n:%f e:%f d:%f",arg1,pos_n,pos_e,pos_d);
       
        return 1;
    }   //emd of $CMD
    
    if(!strncmp(msg, "$RUN", 4)) {
       
        return 1;
    } 
    if(!strncmp(msg, "$STOP", 4)) {
       
        return 1;
    }     

    
    return 0;

}