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@8:0b7925095416, 2017-10-20 (annotated)

Committer:
Alex_Kyrl
Date:
Fri Oct 20 14:38:14 2017 +0000
Revision:
8:0b7925095416
Parent:
6:452e301a105a
Child:
9:22d79a4a0324
change motor to be angle control ;

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