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@6:452e301a105a, 2017-10-16 (annotated)

Committer:
Alex_Kyrl
Date:
Mon Oct 16 13:07:53 2017 +0000
Revision:
6:452e301a105a
Child:
8:0b7925095416
using class for emg and added motor class

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 velocity = 0;
Alex_Kyrl 6:452e301a105a 7 _PWM=0;
Alex_Kyrl 6:452e301a105a 8 _Encoder.QEI::reset();
Alex_Kyrl 6:452e301a105a 9 frequency=50000;
Alex_Kyrl 6:452e301a105a 10 set_period(frequency);
Alex_Kyrl 6:452e301a105a 11 angle=0;
Alex_Kyrl 6:452e301a105a 12 angle_prev =0;
Alex_Kyrl 6:452e301a105a 13 sample_time = 0.002;
Alex_Kyrl 6:452e301a105a 14 safety_angle = 90; //safety angle in DEGREES
Alex_Kyrl 6:452e301a105a 15 POS=1 , NEG=0;
Alex_Kyrl 6:452e301a105a 16 _direction= 1;
Alex_Kyrl 6:452e301a105a 17 }
Alex_Kyrl 6:452e301a105a 18
Alex_Kyrl 6:452e301a105a 19 Motor::Motor(float vel , PinName a , PinName b , PinName c , PinName direction , PinName PWM , int pulse , float freq , int dir , float samp , float safe , int pos) : _Encoder(a,b,c,pulse), _direction(direction) , _PWM(PWM)
Alex_Kyrl 6:452e301a105a 20 {
Alex_Kyrl 6:452e301a105a 21 velocity = vel;
Alex_Kyrl 6:452e301a105a 22 _PWM=0;
Alex_Kyrl 6:452e301a105a 23 _Encoder.QEI::reset();
Alex_Kyrl 6:452e301a105a 24 frequency=freq;
Alex_Kyrl 6:452e301a105a 25 set_period(frequency);
Alex_Kyrl 6:452e301a105a 26 angle= 0;
Alex_Kyrl 6:452e301a105a 27 angle_prev = 0;
Alex_Kyrl 6:452e301a105a 28 sample_time = samp;
Alex_Kyrl 6:452e301a105a 29 safety_angle = safe;
Alex_Kyrl 6:452e301a105a 30 POS = pos;
Alex_Kyrl 6:452e301a105a 31 NEG = !pos;
Alex_Kyrl 6:452e301a105a 32 _direction=POS;
Alex_Kyrl 6:452e301a105a 33 }
Alex_Kyrl 6:452e301a105a 34
Alex_Kyrl 6:452e301a105a 35 void Motor::change_direction(){
Alex_Kyrl 6:452e301a105a 36
Alex_Kyrl 6:452e301a105a 37 _direction=!_direction; //should probably have some call to DigitalOut:: maybe can't use overloaded operators here
Alex_Kyrl 6:452e301a105a 38
Alex_Kyrl 6:452e301a105a 39 }
Alex_Kyrl 6:452e301a105a 40
Alex_Kyrl 6:452e301a105a 41 void Motor::set_period(float freq){
Alex_Kyrl 6:452e301a105a 42 frequency=freq;
Alex_Kyrl 6:452e301a105a 43 _PWM.period(1/frequency);
Alex_Kyrl 6:452e301a105a 44
Alex_Kyrl 6:452e301a105a 45 }
Alex_Kyrl 6:452e301a105a 46 void Motor::set_velocity(){
Alex_Kyrl 6:452e301a105a 47
Alex_Kyrl 6:452e301a105a 48 velocity = (angle-angle_prev)/sample_time; // differentiate angle TODO: needs filtering!
Alex_Kyrl 6:452e301a105a 49 angle_prev = angle;
Alex_Kyrl 6:452e301a105a 50
Alex_Kyrl 6:452e301a105a 51 }
Alex_Kyrl 6:452e301a105a 52
Alex_Kyrl 6:452e301a105a 53 void Motor::set_angle(){
Alex_Kyrl 6:452e301a105a 54
Alex_Kyrl 6:452e301a105a 55 int n_pulse= _Encoder.QEI::getPulses();
Alex_Kyrl 6:452e301a105a 56
Alex_Kyrl 6:452e301a105a 57 angle = ((n_pulse)*(360/32))/131; // get angle TODO: Change 32 to 4th value of encoder
Alex_Kyrl 6:452e301a105a 58 cout<<angle<<endl;
Alex_Kyrl 6:452e301a105a 59 }
Alex_Kyrl 6:452e301a105a 60
Alex_Kyrl 6:452e301a105a 61 void Motor::control_velocity(float vel){
Alex_Kyrl 6:452e301a105a 62
Alex_Kyrl 6:452e301a105a 63 float control_velocity=vel;
Alex_Kyrl 6:452e301a105a 64
Alex_Kyrl 6:452e301a105a 65 //float error = velocity - control_velocity; //set error probably need some kind of PID
Alex_Kyrl 6:452e301a105a 66
Alex_Kyrl 6:452e301a105a 67 if(angle >= (safety_angle-3) and _direction == !POS)
Alex_Kyrl 6:452e301a105a 68 {
Alex_Kyrl 6:452e301a105a 69 _PWM = 0;
Alex_Kyrl 6:452e301a105a 70 }
Alex_Kyrl 6:452e301a105a 71 else if(angle <= -(safety_angle-3) and _direction == !NEG)
Alex_Kyrl 6:452e301a105a 72 {
Alex_Kyrl 6:452e301a105a 73 _PWM = 0;
Alex_Kyrl 6:452e301a105a 74 }
Alex_Kyrl 6:452e301a105a 75 else
Alex_Kyrl 6:452e301a105a 76 {
Alex_Kyrl 6:452e301a105a 77 _PWM = control_velocity ;
Alex_Kyrl 6:452e301a105a 78 }
Alex_Kyrl 6:452e301a105a 79
Alex_Kyrl 6:452e301a105a 80
Alex_Kyrl 6:452e301a105a 81 }
Alex_Kyrl 6:452e301a105a 82
Alex_Kyrl 6:452e301a105a 83 float Motor::get_velocity(){
Alex_Kyrl 6:452e301a105a 84
Alex_Kyrl 6:452e301a105a 85 return velocity;
Alex_Kyrl 6:452e301a105a 86 }
Alex_Kyrl 6:452e301a105a 87
Alex_Kyrl 6:452e301a105a 88 float Motor::get_period(){
Alex_Kyrl 6:452e301a105a 89
Alex_Kyrl 6:452e301a105a 90 return 1/frequency;
Alex_Kyrl 6:452e301a105a 91
Alex_Kyrl 6:452e301a105a 92 }