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@11:dd1976534a03, 2017-10-30 (annotated)

Committer:
Alex_Kyrl
Date:
Mon Oct 30 15:32:27 2017 +0000
Revision:
11:dd1976534a03
Parent:
9:22d79a4a0324
Child:
12:69a9cf74583e
Testing motor PWM min

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 11:dd1976534a03 10 angle=45;
Alex_Kyrl 8:0b7925095416 11 safety_angle = 120; //safety angle in DEGREES
Alex_Kyrl 8:0b7925095416 12 _direction= 1;
Alex_Kyrl 8:0b7925095416 13 low_PWM = 0.6;
Alex_Kyrl 6:452e301a105a 14 }
Alex_Kyrl 6:452e301a105a 15
Alex_Kyrl 9:22d79a4a0324 16 Motor::Motor(PinName a , PinName b, PinName direction , PinName PWM , float freq , float safe , float low ) : _Encoder(a,b,NC,32), _direction(direction) , _PWM(PWM)
Alex_Kyrl 6:452e301a105a 17 {
Alex_Kyrl 6:452e301a105a 18 _PWM=0;
Alex_Kyrl 6:452e301a105a 19 _Encoder.QEI::reset();
Alex_Kyrl 6:452e301a105a 20 frequency=freq;
Alex_Kyrl 6:452e301a105a 21 set_period(frequency);
Alex_Kyrl 11:dd1976534a03 22 angle= 45;
Alex_Kyrl 6:452e301a105a 23 safety_angle = safe;
Alex_Kyrl 8:0b7925095416 24 _direction= 1;
Alex_Kyrl 8:0b7925095416 25 low_PWM= low;
Alex_Kyrl 6:452e301a105a 26 }
Alex_Kyrl 6:452e301a105a 27
Alex_Kyrl 6:452e301a105a 28
Alex_Kyrl 6:452e301a105a 29 void Motor::set_period(float freq){
Alex_Kyrl 6:452e301a105a 30 frequency=freq;
Alex_Kyrl 6:452e301a105a 31 _PWM.period(1/frequency);
Alex_Kyrl 6:452e301a105a 32
Alex_Kyrl 6:452e301a105a 33 }
Alex_Kyrl 8:0b7925095416 34
Alex_Kyrl 8:0b7925095416 35 float Motor::set_angle(){
Alex_Kyrl 6:452e301a105a 36
Alex_Kyrl 8:0b7925095416 37 int n_pulse= _Encoder.QEI::getPulses();
Alex_Kyrl 6:452e301a105a 38
Alex_Kyrl 11:dd1976534a03 39 angle = 45 + ((n_pulse)*(360/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 40 return angle;
Alex_Kyrl 6:452e301a105a 41 }
Alex_Kyrl 6:452e301a105a 42
Alex_Kyrl 11:dd1976534a03 43 double Motor::Control_angle(float ang){
Alex_Kyrl 6:452e301a105a 44
Alex_Kyrl 8:0b7925095416 45 float control_angle=ang;
Alex_Kyrl 11:dd1976534a03 46 //float step = 360/(32*131);
Alex_Kyrl 9:22d79a4a0324 47 //control_angle = control_angle - fmodf(control_angle, step) ; //changle control angle to posible angles of the sensor
Alex_Kyrl 6:452e301a105a 48
Alex_Kyrl 11:dd1976534a03 49 if(control_angle > safety_angle+45)
Alex_Kyrl 11:dd1976534a03 50 control_angle = safety_angle+45;
Alex_Kyrl 11:dd1976534a03 51 if(control_angle < -safety_angle+45)
Alex_Kyrl 11:dd1976534a03 52 control_angle = -safety_angle+45;
Alex_Kyrl 8:0b7925095416 53
Alex_Kyrl 9:22d79a4a0324 54 double error = angle - control_angle; //set error probably need some kind of PID
Alex_Kyrl 8:0b7925095416 55
Alex_Kyrl 8:0b7925095416 56 if(error >= 0)
Alex_Kyrl 8:0b7925095416 57 _direction = 1;
Alex_Kyrl 6:452e301a105a 58 else
Alex_Kyrl 11:dd1976534a03 59 _direction = 0;
Alex_Kyrl 11:dd1976534a03 60
Alex_Kyrl 11:dd1976534a03 61 if( 0.1 < fabs(error) < low_PWM)
Alex_Kyrl 8:0b7925095416 62 _PWM = low_PWM;
Alex_Kyrl 11:dd1976534a03 63 else if(fabs(error) >= 1 )
Alex_Kyrl 8:0b7925095416 64 _PWM = 1;
Alex_Kyrl 11:dd1976534a03 65 else if (fabs(error) < 0.1)
Alex_Kyrl 9:22d79a4a0324 66 _PWM = 0;
Alex_Kyrl 8:0b7925095416 67 else
Alex_Kyrl 11:dd1976534a03 68 _PWM = fabs(error);
Alex_Kyrl 11:dd1976534a03 69
Alex_Kyrl 11:dd1976534a03 70 return error;
Alex_Kyrl 8:0b7925095416 71
Alex_Kyrl 8:0b7925095416 72 }