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

01_DriveTrain/pidBearing.h

Committer:
ahmed_lv
Date:
2018-03-20
Revision:
30:44676e1b38f8
Parent:
29:a6a812ee83ea

File content as of revision 30:44676e1b38f8:

#ifndef pidBearing_H
#define pidBearing_H

#include "generalFunctions.h"
#include "mbed.h"
#include "orion_pinmapping.h"
#include "config.h"
#include "math.h"

class pidBearing
{
public:
    //motorDriver(PinName mPlus, PinName mMinus, int freq_khz);
    //void setPWM(float val);
    
    pidBearing();
    void setSpeedChange(float* wl, float* wr, float* speedChange, float* Error, 
                                float target[2], 
                                float local[2],
                                float yaw, 
                                float* robotFrame, 
                                float xx, float yy, 
                                float* k3,
                                float lGain);                                
    void pidBearing::findRobotFrameDistance(float target[2], float local[2], float* robotFrame); 

private:
    float kp, kd, ki;
    float imuToAchieve, offset, angleToRotate, prevaError, bearingToWaypoint, sumError, max_in, minDist;
};


#endif