190711
Diff: motor.cpp
- Revision:
- 4:a629f46d6594
- Parent:
- 3:df8469e71728
- Child:
- 5:15d0276d7e21
diff -r df8469e71728 -r a629f46d6594 motor.cpp --- a/motor.cpp Thu May 23 08:53:19 2019 +0000 +++ b/motor.cpp Thu May 23 23:02:14 2019 +0000 @@ -8,8 +8,8 @@ _pwm.period_ms(32); _pwm.write(0); _tick.attach_us(this,&MotorCtl::PIDControl,32000); - kp=0.2; - ki=0.05; + kp=0.1; + ki=0.2; kd=0.0; integ=0; derv=0; @@ -19,7 +19,7 @@ CntPerRev = 2400; // 100 shft/rev x 6 pls/shft x 4 evnt/pls duty = 50; PreviousEncode = 0; - TargetSpeed = 50; //initial target speed + TargetSpeed = 0; //initial target speed } @@ -139,29 +139,30 @@ DeltaCnt = CurrentPosition - PreviousPosition; PreviousPosition = CurrentPosition; CurrentSpeed = (int)CalculateRPM(DeltaCnt); - printf("DeltaCnt: %d CurrentSpeed: %ld\r\n",DeltaCnt,CurrentSpeed); +// printf("DeltaCnt: %d CurrentSpeed: %ld\r\n",DeltaCnt,CurrentSpeed); Error = TargetSpeed - CurrentSpeed; integ += Error; derv = Error-PreviousError; control = (int)(kp*Error+ki*integ+kd*derv); - if (control>35) control=35; - else if(control <-35) control = -35; + if (control>40) control=40; + else if(control <-40) control = -40; PreviousError = Error; if(TargetSpeed>0){ - duty =TargetSpeed+control; + duty =(TargetSpeed+control)/200.0; + _pwm.write(duty); _Dir=1; - //printf("control: %d\r\n",control); +// printf("control: %d\r\n",control); }else{ - duty =TargetSpeed-control; + duty =(-TargetSpeed-control)/200.0; + _pwm.write(duty); _Dir=0; } - //if(duty<=0){duty=-duty;} - vel=duty/180.0; + - printf("Duty: %d vel: %f\r\n",duty,vel); - _pwm.write(vel); +// printf("Duty: %d vel: %f\r\n",duty,vel); + @@ -173,7 +174,7 @@ float MotorCtl::CalculateRPM(int DeltaCnt) { - return(DeltaCnt*60*1000)/(DeltaT*CntPerRev); // Time measured in us + return(DeltaCnt*60.0*1000.0)/(float)(DeltaT*CntPerRev); // Time measured in us }