Ya kno it

Dependencies:   PID QEI_adapted

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Motor.cpp Source File

Motor.cpp

00001 #include "Motor.h"
00002 
00003 Motor::Motor(PinName PWM, PinName Direction, PinName Enc1,PinName Enc2,PinName HomingSW,float interval):
00004     encoder(Enc1,Enc2,NC,4200),
00005     MotorThrottle(PWM),
00006     MotorDirection(Direction),
00007     HomingSwitch(HomingSW),
00008     controller(1,0,0,interval)
00009 {
00010     SetPID(1,0,0);
00011     _GearRatio=1;
00012 }
00013 
00014 void Motor::GotoPos(float Rad){
00015     // Enter The current values
00016     controller.setSetPoint(Rad);
00017     controller.setProcessValue(GetPos());
00018     
00019     // Compute controller output
00020     float OutPut=controller.compute();
00021     
00022     // Direction handling
00023     float Direction=0;
00024     
00025     if(OutPut<0){
00026         Direction=1;
00027     }
00028     
00029     OutPut=fabs(OutPut);
00030     
00031     // Output schrijven
00032     MotorThrottle.write(OutPut);
00033     MotorDirection=Direction;
00034 }
00035 
00036 float Motor::GetPos(){
00037     return (encoder.getPulses()/4200.0f)*(2.0f*3.1415f)/_GearRatio;
00038 }
00039 
00040 
00041 void Motor::Homing(bool direction, float PWM,float HomingPos){
00042     HomingSwitch.mode(PullUp);
00043     MotorThrottle=PWM;
00044     MotorDirection=direction;
00045     while (HomingSwitch.read()==1){
00046     }
00047     MotorDirection=0;
00048     MotorThrottle=0.0f;
00049     encoder.reset((int)4200.0f*_GearRatio*(HomingPos/2.0f/3.1415));
00050 }
00051 
00052 void Motor::SetPID(float P,float Ti,float Td){
00053     controller.setTunings(P,Ti, Td);
00054 }
00055 
00056 void Motor::SetGearRatio(float GearRatio){
00057     _GearRatio=GearRatio;
00058 }
00059 
00060 void Motor::SetInputLimits(float Imin, float Imax){
00061     controller.setInputLimits(Imin,Imax);
00062 }
00063 
00064 void Motor::SetOutputLimits(float Omin, float Omax){
00065     controller.setInputLimits(Omin,Omax);
00066 }
00067 
00068 void Motor::Stop(){
00069     MotorThrottle=0;
00070     MotorDirection=0;
00071 }
00072     
00073 
00074 
00075 
00076 
00077 
00078 
00079