Team Virgo v3 / Orion_newPCB_test_LV

Implement new controller

Dependencies:   mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler_Orion_PCB MAX17048 Servo

Fork of Orion_newPCB_test by Team Virgo v3

05_TrajectoryTracking/kinematics.cpp

Committer:
akashvibhute
Date:
2016-04-23
Revision:
10:ea4b90be68dc

File content as of revision 10:ea4b90be68dc:

#include "kinematics.h"

kinematics::kinematics()
{
    wheelDia = wheel_dia;
    trackWidth = track_width;

}

void kinematics::VW_RPM(float velocity, float omega, float* left_rpm, float* right_rpm)
{
    *right_rpm = (velocity + (omega * trackWidth / 2)) * (60.0 / (M_PI * wheelDia));
    *left_rpm = (velocity - (omega * trackWidth / 2)) * (60.0 / (M_PI * wheelDia));

    /*
    if(*right_rpm > 100)
        *right_rpm = generalFunctions::constrain_f(*right_rpm, min_rpm, max_rpm);
    else
        *right_rpm = 0;

    if(*left_rpm > 100)
        *left_rpm = generalFunctions::constrain_f(*left_rpm, min_rpm, max_rpm);
    else
        *left_rpm = 0;    
    */
}