Poep Hoofd / Mbed 2 deprecated PoolRobot_Code

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope mbed MODSERIAL QEI

Motor.cpp@16:a2a73d57d556, 2017-11-02 (annotated)

Committer:
john111222333
Date:
Thu Nov 02 15:07:53 2017 +0000
Revision:
16:a2a73d57d556
Parent:
14:f561498eee28
Child:
21:59431788a42d
not working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Alex_Kyrl 6:452e301a105a 1 #include "Motor.h"
Alex_Kyrl 6:452e301a105a 2
Alex_Kyrl 6:452e301a105a 3
Alex_Kyrl 6:452e301a105a 4 Motor::Motor() : _Encoder(D13,D12,NC,32), _direction(D7) , _PWM(D6)
Alex_Kyrl 6:452e301a105a 5 {
Alex_Kyrl 6:452e301a105a 6 _PWM=0;
Alex_Kyrl 6:452e301a105a 7 _Encoder.QEI::reset();
Alex_Kyrl 6:452e301a105a 8 frequency=50000;
Alex_Kyrl 6:452e301a105a 9 set_period(frequency);
Alex_Kyrl 12:69a9cf74583e 10 angle=90;
Alex_Kyrl 8:0b7925095416 11 safety_angle = 120; //safety angle in DEGREES
Alex_Kyrl 13:559f8946f16d 12 POS=1;
Alex_Kyrl 13:559f8946f16d 13 _direction= POS;
Alex_Kyrl 8:0b7925095416 14 low_PWM = 0.6;
Alex_Kyrl 13:559f8946f16d 15 initial_angle = 90;
john111222333 16:a2a73d57d556 16 Ki = 2.0 ;
john111222333 16:a2a73d57d556 17 Kd = 0.5 ;
john111222333 16:a2a73d57d556 18 Kp = 3;
Alex_Kyrl 6:452e301a105a 19 }
Alex_Kyrl 6:452e301a105a 20
john111222333 16:a2a73d57d556 21 Motor::Motor(PinName a , PinName b, PinName direction , PinName PWM , float freq , float safe , float low , int pos , int ini , float p , float i , float d) : _Encoder(a,b,NC,32), _direction(direction) , _PWM(PWM)
Alex_Kyrl 6:452e301a105a 22 {
Alex_Kyrl 6:452e301a105a 23 _PWM=0;
Alex_Kyrl 6:452e301a105a 24 _Encoder.QEI::reset();
Alex_Kyrl 6:452e301a105a 25 frequency=freq;
Alex_Kyrl 6:452e301a105a 26 set_period(frequency);
Alex_Kyrl 13:559f8946f16d 27 initial_angle = ini ;
Alex_Kyrl 13:559f8946f16d 28 angle = initial_angle;
Alex_Kyrl 13:559f8946f16d 29 safety_angle = safe;
Alex_Kyrl 13:559f8946f16d 30 POS = pos;
Alex_Kyrl 13:559f8946f16d 31 _direction= POS;
Alex_Kyrl 8:0b7925095416 32 low_PWM= low;
john111222333 16:a2a73d57d556 33 Kp = p;
john111222333 16:a2a73d57d556 34 Ki = i;
john111222333 16:a2a73d57d556 35 Kd = d;
john111222333 16:a2a73d57d556 36
Alex_Kyrl 6:452e301a105a 37 }
Alex_Kyrl 6:452e301a105a 38
Alex_Kyrl 6:452e301a105a 39
Alex_Kyrl 6:452e301a105a 40 void Motor::set_period(float freq){
Alex_Kyrl 6:452e301a105a 41 frequency=freq;
Alex_Kyrl 6:452e301a105a 42 _PWM.period(1/frequency);
Alex_Kyrl 6:452e301a105a 43
Alex_Kyrl 13:559f8946f16d 44 }
Alex_Kyrl 8:0b7925095416 45
Alex_Kyrl 8:0b7925095416 46 float Motor::set_angle(){
Alex_Kyrl 6:452e301a105a 47
Alex_Kyrl 8:0b7925095416 48 int n_pulse= _Encoder.QEI::getPulses();
Alex_Kyrl 13:559f8946f16d 49 angle = initial_angle - n_pulse*360.0f/(32*131); // get angle (131 is the gear ratio of the motor in order to work in degrees of the actual arm)
Alex_Kyrl 8:0b7925095416 50 return angle;
Alex_Kyrl 6:452e301a105a 51 }
Alex_Kyrl 6:452e301a105a 52
john111222333 14:f561498eee28 53 double Motor::Control_PID(float error){
john111222333 14:f561498eee28 54
john111222333 14:f561498eee28 55 static double int_error ;
john111222333 14:f561498eee28 56 double der_error ;
john111222333 14:f561498eee28 57 double sample_time = 0.002 ;
john111222333 14:f561498eee28 58 double prev_error = 0 ;
john111222333 16:a2a73d57d556 59 double ScaleVal = 1;//Kp/(360.0*5); //For initial testing purposes, set scaling so that maximum error (360 * M1_KP) equals 100% Duty Cycle
john111222333 14:f561498eee28 60
john111222333 14:f561498eee28 61 der_error = (error-prev_error)/sample_time;
john111222333 14:f561498eee28 62
john111222333 14:f561498eee28 63 int_error += sample_time*error;
john111222333 14:f561498eee28 64
john111222333 14:f561498eee28 65 prev_error = error ;
john111222333 14:f561498eee28 66 return (Kp*error + Ki*int_error + Kd*der_error)*ScaleVal;
john111222333 14:f561498eee28 67
john111222333 14:f561498eee28 68
john111222333 14:f561498eee28 69 }
john111222333 14:f561498eee28 70
Alex_Kyrl 13:559f8946f16d 71 float Motor::Control_angle(float ang){
Alex_Kyrl 6:452e301a105a 72
Alex_Kyrl 8:0b7925095416 73 float control_angle=ang;
john111222333 14:f561498eee28 74 double error = Control_PID(angle - control_angle); //set error probably need some kind of PID
john111222333 14:f561498eee28 75
john111222333 14:f561498eee28 76
john111222333 14:f561498eee28 77 if(angle - control_angle >= 0)
john111222333 14:f561498eee28 78 _direction = POS;
john111222333 14:f561498eee28 79 else
john111222333 14:f561498eee28 80 _direction = !POS;
john111222333 14:f561498eee28 81
Alex_Kyrl 13:559f8946f16d 82 //float step = 360.0/(32*131);
Alex_Kyrl 13:559f8946f16d 83 // control_angle -= remainder(control_angle , step); //changle control angle to posible angles of the sensor
Alex_Kyrl 13:559f8946f16d 84
john111222333 14:f561498eee28 85 if(control_angle > safety_angle + 90 and error >= 0)
Alex_Kyrl 13:559f8946f16d 86 {
john111222333 14:f561498eee28 87 control_angle = safety_angle + 90;
Alex_Kyrl 13:559f8946f16d 88 _PWM = 0;
Alex_Kyrl 13:559f8946f16d 89 }
john111222333 14:f561498eee28 90 else if(control_angle < -safety_angle + 90 and error <= 0)
Alex_Kyrl 13:559f8946f16d 91 {
john111222333 14:f561498eee28 92 control_angle = -safety_angle + 90;
Alex_Kyrl 13:559f8946f16d 93 _PWM = 0;
Alex_Kyrl 13:559f8946f16d 94 }
Alex_Kyrl 6:452e301a105a 95 else
Alex_Kyrl 13:559f8946f16d 96 {
john111222333 14:f561498eee28 97 if( 0.01 < fabs(error) and fabs(error) < low_PWM)
john111222333 14:f561498eee28 98 _PWM = low_PWM;
john111222333 14:f561498eee28 99 else if( fabs(error) >= 1 )
john111222333 14:f561498eee28 100 _PWM = 1;
john111222333 14:f561498eee28 101 else if( fabs(error) <= 0.01 )
john111222333 14:f561498eee28 102 _PWM = 0;
john111222333 14:f561498eee28 103 else
john111222333 14:f561498eee28 104 _PWM = fabs(error);
john111222333 14:f561498eee28 105 }
Alex_Kyrl 13:559f8946f16d 106
Alex_Kyrl 13:559f8946f16d 107
Alex_Kyrl 11:dd1976534a03 108
Alex_Kyrl 13:559f8946f16d 109 return _PWM;
Alex_Kyrl 8:0b7925095416 110
Alex_Kyrl 8:0b7925095416 111 }