Alexandros Kyrloglou / Mbed 2 deprecated BioRobotics_pool_bot

Motor class working

Dependencies:   QEI biquadFilter mbed

Motor.cpp@4:38c653bfec5f, 2017-10-16 (annotated)

Committer:
Alex_Kyrl
Date:
Mon Oct 16 12:34:11 2017 +0000
Revision:
4:38c653bfec5f
Parent:
3:9d861827b714 
added EMG class;

Who changed what in which revision?

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