prototype encoder launcher
Dependencies: Motor mbed millis
main.cpp@0:bc9c90c7b357, 2017-02-08 (annotated)
- Committer:
- Najib_irvani
- Date:
- Wed Feb 08 13:55:01 2017 +0000
- Revision:
- 0:bc9c90c7b357
prototype encoder launcher
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Najib_irvani | 0:bc9c90c7b357 | 1 | #include "mbed.h" |
Najib_irvani | 0:bc9c90c7b357 | 2 | #include "encoderKRAI.h" |
Najib_irvani | 0:bc9c90c7b357 | 3 | #include "Motor.h" |
Najib_irvani | 0:bc9c90c7b357 | 4 | #include "millis.h" |
Najib_irvani | 0:bc9c90c7b357 | 5 | |
Najib_irvani | 0:bc9c90c7b357 | 6 | double speed, maxSpeed = 0.8, minSpeed = -0.8, last_speed = 0.0; |
Najib_irvani | 0:bc9c90c7b357 | 7 | double kp=0.0005, kd=0.005, ki=0.000005; |
Najib_irvani | 0:bc9c90c7b357 | 8 | double p,i,d; |
Najib_irvani | 0:bc9c90c7b357 | 9 | double last_error = 0, current_error, sum_error = 0; |
Najib_irvani | 0:bc9c90c7b357 | 10 | double rpm, target_rpm = 1500.0; |
Najib_irvani | 0:bc9c90c7b357 | 11 | |
Najib_irvani | 0:bc9c90c7b357 | 12 | encoderKRAI encoder( PB_14, PB_13, 14, encoderKRAI::X2_ENCODING); |
Najib_irvani | 0:bc9c90c7b357 | 13 | Serial pc(USBTX,USBRX); |
Najib_irvani | 0:bc9c90c7b357 | 14 | Motor motor(PA_8,PC_1,PC_2); |
Najib_irvani | 0:bc9c90c7b357 | 15 | |
Najib_irvani | 0:bc9c90c7b357 | 16 | unsigned long int previousMillis = 0; |
Najib_irvani | 0:bc9c90c7b357 | 17 | unsigned long int currentMillis; |
Najib_irvani | 0:bc9c90c7b357 | 18 | |
Najib_irvani | 0:bc9c90c7b357 | 19 | void init_speed (){ |
Najib_irvani | 0:bc9c90c7b357 | 20 | if (speed>maxSpeed){ |
Najib_irvani | 0:bc9c90c7b357 | 21 | speed = maxSpeed; |
Najib_irvani | 0:bc9c90c7b357 | 22 | } |
Najib_irvani | 0:bc9c90c7b357 | 23 | |
Najib_irvani | 0:bc9c90c7b357 | 24 | if (speed<minSpeed){ |
Najib_irvani | 0:bc9c90c7b357 | 25 | speed = minSpeed; |
Najib_irvani | 0:bc9c90c7b357 | 26 | } |
Najib_irvani | 0:bc9c90c7b357 | 27 | } |
Najib_irvani | 0:bc9c90c7b357 | 28 | |
Najib_irvani | 0:bc9c90c7b357 | 29 | |
Najib_irvani | 0:bc9c90c7b357 | 30 | |
Najib_irvani | 0:bc9c90c7b357 | 31 | |
Najib_irvani | 0:bc9c90c7b357 | 32 | int main() { |
Najib_irvani | 0:bc9c90c7b357 | 33 | |
Najib_irvani | 0:bc9c90c7b357 | 34 | startMillis(); |
Najib_irvani | 0:bc9c90c7b357 | 35 | |
Najib_irvani | 0:bc9c90c7b357 | 36 | |
Najib_irvani | 0:bc9c90c7b357 | 37 | while(1) { |
Najib_irvani | 0:bc9c90c7b357 | 38 | current_error = target_rpm - rpm; |
Najib_irvani | 0:bc9c90c7b357 | 39 | sum_error += current_error; |
Najib_irvani | 0:bc9c90c7b357 | 40 | p = current_error*kp; |
Najib_irvani | 0:bc9c90c7b357 | 41 | d = (current_error-last_error)*kd; |
Najib_irvani | 0:bc9c90c7b357 | 42 | i = sum_error*ki; |
Najib_irvani | 0:bc9c90c7b357 | 43 | |
Najib_irvani | 0:bc9c90c7b357 | 44 | speed = last_speed + p + d; |
Najib_irvani | 0:bc9c90c7b357 | 45 | init_speed(); |
Najib_irvani | 0:bc9c90c7b357 | 46 | motor.speed(speed); |
Najib_irvani | 0:bc9c90c7b357 | 47 | currentMillis = millis(); |
Najib_irvani | 0:bc9c90c7b357 | 48 | |
Najib_irvani | 0:bc9c90c7b357 | 49 | |
Najib_irvani | 0:bc9c90c7b357 | 50 | if (currentMillis-previousMillis>=50) |
Najib_irvani | 0:bc9c90c7b357 | 51 | { |
Najib_irvani | 0:bc9c90c7b357 | 52 | rpm = (double)encoder.getPulses()*60000/(double)(14.0*50.0); |
Najib_irvani | 0:bc9c90c7b357 | 53 | previousMillis = currentMillis; |
Najib_irvani | 0:bc9c90c7b357 | 54 | encoder.reset(); |
Najib_irvani | 0:bc9c90c7b357 | 55 | pc.printf("%.04lf\n",rpm); |
Najib_irvani | 0:bc9c90c7b357 | 56 | |
Najib_irvani | 0:bc9c90c7b357 | 57 | } |
Najib_irvani | 0:bc9c90c7b357 | 58 | //last_speed = speed; |
Najib_irvani | 0:bc9c90c7b357 | 59 | last_error = current_error; |
Najib_irvani | 0:bc9c90c7b357 | 60 | |
Najib_irvani | 0:bc9c90c7b357 | 61 | } |
Najib_irvani | 0:bc9c90c7b357 | 62 | } |