20160814

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

Committer:
adam_z
Date:
Fri Apr 22 09:39:01 2016 +0000
Revision:
0:955aa05c968a
Child:
1:c5973a56d474
started;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
adam_z 0:955aa05c968a 1 #include "mbed.h"
adam_z 0:955aa05c968a 2 #include "CURRENT_CONTROL.h"
adam_z 0:955aa05c968a 3
adam_z 0:955aa05c968a 4
adam_z 0:955aa05c968a 5 void CURRENT_CONTROL::CURRENT_CONTROL(PinName curChannel,
adam_z 0:955aa05c968a 6 PinName PwmChannel1,
adam_z 0:955aa05c968a 7 PinName PwmChannel2,
adam_z 0:955aa05c968a 8 PWMIndex pwmIndex,
adam_z 0:955aa05c968a 9 float Kp, float Ki, float Kd,
adam_z 0:955aa05c968a 10 float samplingTime):
adam_z 0:955aa05c968a 11 currentAnalogIn(curChannel),
adam_z 0:955aa05c968a 12 MotorPlus(PwmChannel1),
adam_z 0:955aa05c968a 13 MotorMinus(PwmChannel2),
adam_z 0:955aa05c968a 14 pid(Kp,Ki,Kd,samplingTime)
adam_z 0:955aa05c968a 15 {
adam_z 0:955aa05c968a 16 PWMIndex = pwmIndex;
adam_z 0:955aa05c968a 17 Ts = samplingTime;
adam_z 0:955aa05c968a 18 //setup motor PWM parameters
adam_z 0:955aa05c968a 19 MotorPlus.period_us(50);//period equals to 50 microseconds
adam_z 0:955aa05c968a 20 MotorPlus.write(0.5); //duty ratio = 0.5 in complementary output -> static
adam_z 0:955aa05c968a 21 TIM1->CCER |= 4; //enable ch1 complimentary output
adam_z 0:955aa05c968a 22
adam_z 0:955aa05c968a 23 }
adam_z 0:955aa05c968a 24
adam_z 0:955aa05c968a 25 void CURRENT_CONTROL::ControlCompute(float curRef)
adam_z 0:955aa05c968a 26 {
adam_z 0:955aa05c968a 27 curFeedBack = (currentAnalogIn.read() - currentOffset)*3.3*8/0.6;
adam_z 0:955aa05c968a 28 pid.Compute(curRef, curFeedBack);
adam_z 0:955aa05c968a 29 MotorPlus = 0.5 - pid.output;
adam_z 0:955aa05c968a 30 if(PWMIndex == PWM1)TIM1->CCER |= 4;
adam_z 0:955aa05c968a 31 else if(PWMIndex == PWM2)TIM1->CCER |= 64;
adam_z 0:955aa05c968a 32 }