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@14:f561498eee28, 2017-11-01 (annotated)

Committer:
john111222333
Date:
Wed Nov 01 13:08:04 2017 +0000
Revision:
14:f561498eee28
Parent:
13:559f8946f16d
Child:
16:a2a73d57d556
working program with kinematics (tune PID needed)

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