prototype encoder launcher
Dependencies: Motor mbed millis
Diff: main.cpp
- Revision:
- 0:bc9c90c7b357
diff -r 000000000000 -r bc9c90c7b357 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Feb 08 13:55:01 2017 +0000 @@ -0,0 +1,62 @@ +#include "mbed.h" +#include "encoderKRAI.h" +#include "Motor.h" +#include "millis.h" + +double speed, maxSpeed = 0.8, minSpeed = -0.8, last_speed = 0.0; +double kp=0.0005, kd=0.005, ki=0.000005; +double p,i,d; +double last_error = 0, current_error, sum_error = 0; +double rpm, target_rpm = 1500.0; + +encoderKRAI encoder( PB_14, PB_13, 14, encoderKRAI::X2_ENCODING); +Serial pc(USBTX,USBRX); +Motor motor(PA_8,PC_1,PC_2); + +unsigned long int previousMillis = 0; +unsigned long int currentMillis; + +void init_speed (){ + if (speed>maxSpeed){ + speed = maxSpeed; + } + + if (speed<minSpeed){ + speed = minSpeed; + } +} + + + + +int main() { + +startMillis(); + + + while(1) { + current_error = target_rpm - rpm; + sum_error += current_error; + p = current_error*kp; + d = (current_error-last_error)*kd; + i = sum_error*ki; + + speed = last_speed + p + d; + init_speed(); + motor.speed(speed); + currentMillis = millis(); + + + if (currentMillis-previousMillis>=50) + { + rpm = (double)encoder.getPulses()*60000/(double)(14.0*50.0); + previousMillis = currentMillis; + encoder.reset(); + pc.printf("%.04lf\n",rpm); + + } + //last_speed = speed; + last_error = current_error; + + } +}