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@13:559f8946f16d, 2017-11-01 (annotated)

Committer:
Alex_Kyrl
Date:
Wed Nov 01 07:11:00 2017 +0000
Revision:
13:559f8946f16d
Parent:
12:69a9cf74583e
Child:
14:f561498eee28
test why it doesn't work

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
Alex_Kyrl 13:559f8946f16d 46 float Motor::Control_angle(float ang){
Alex_Kyrl 6:452e301a105a 47
Alex_Kyrl 8:0b7925095416 48 float control_angle=ang;
Alex_Kyrl 13:559f8946f16d 49 //float step = 360.0/(32*131);
Alex_Kyrl 13:559f8946f16d 50 // control_angle -= remainder(control_angle , step); //changle control angle to posible angles of the sensor
Alex_Kyrl 13:559f8946f16d 51
Alex_Kyrl 13:559f8946f16d 52 if(control_angle > safety_angle + initial_angle )
Alex_Kyrl 13:559f8946f16d 53 {
Alex_Kyrl 13:559f8946f16d 54 control_angle = safety_angle + initial_angle;
Alex_Kyrl 13:559f8946f16d 55 _PWM = 0;
Alex_Kyrl 13:559f8946f16d 56 }
Alex_Kyrl 13:559f8946f16d 57 else if(control_angle < -safety_angle + initial_angle)
Alex_Kyrl 13:559f8946f16d 58 {
Alex_Kyrl 13:559f8946f16d 59 control_angle = -safety_angle + initial_angle;
Alex_Kyrl 13:559f8946f16d 60 _PWM = 0;
Alex_Kyrl 13:559f8946f16d 61 }
Alex_Kyrl 6:452e301a105a 62 else
Alex_Kyrl 13:559f8946f16d 63 {
Alex_Kyrl 13:559f8946f16d 64
Alex_Kyrl 13:559f8946f16d 65 double error = angle - control_angle; //set error probably need some kind of PID
Alex_Kyrl 13:559f8946f16d 66
Alex_Kyrl 13:559f8946f16d 67 if(error >= 0)
Alex_Kyrl 13:559f8946f16d 68 _direction = POS;
Alex_Kyrl 13:559f8946f16d 69 else
Alex_Kyrl 13:559f8946f16d 70 _direction = !POS;
Alex_Kyrl 13:559f8946f16d 71
Alex_Kyrl 11:dd1976534a03 72
Alex_Kyrl 13:559f8946f16d 73 if( 0.01 < fabs(error) and fabs(error) < low_PWM)
Alex_Kyrl 13:559f8946f16d 74 _PWM = low_PWM;
Alex_Kyrl 13:559f8946f16d 75 else if(fabs(error) >= 1 )
Alex_Kyrl 13:559f8946f16d 76 _PWM = 1;
Alex_Kyrl 13:559f8946f16d 77 else if(fabs(error) <= 0.01)
Alex_Kyrl 13:559f8946f16d 78 _PWM = 0;
Alex_Kyrl 13:559f8946f16d 79 else
Alex_Kyrl 13:559f8946f16d 80 _PWM = fabs(error);
Alex_Kyrl 13:559f8946f16d 81 }
Alex_Kyrl 13:559f8946f16d 82 return _PWM;
Alex_Kyrl 8:0b7925095416 83
Alex_Kyrl 8:0b7925095416 84 }