Ya kno it
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Fri Aug 5 2022 07:13:33 by
1.7.2