Motor.cpp

00001 #include "Motor.h"
00002 
00003 
00004 Motor::Motor() : _Encoder(D13,D12,NC,32),  _direction(D7) , _PWM(D6)  
00005 {
00006     velocity = 0;  
00007     _PWM=0;
00008     _Encoder.QEI::reset(); 
00009     frequency=50000;
00010     set_period(frequency);
00011     angle=0;
00012     angle_prev =0;
00013     sample_time = 0.002;  
00014     safety_angle = 90; //safety angle in DEGREES
00015     POS=1 , NEG=0;
00016     _direction= 1; 
00017 }
00018 
00019 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)  
00020 {
00021     velocity = vel;  
00022     _PWM=0;
00023     _Encoder.QEI::reset(); 
00024     frequency=freq;
00025     set_period(frequency);
00026     angle= 0;
00027     angle_prev = 0; 
00028     sample_time = samp;
00029     safety_angle = safe;    
00030     POS = pos;
00031     NEG = !pos;
00032     _direction=POS;
00033 }
00034 
00035 void Motor::change_direction(){
00036     
00037     _direction=!_direction; //should probably have some call to DigitalOut:: maybe can't use overloaded operators here
00038 
00039 }
00040 
00041 void Motor::set_period(float freq){
00042     frequency=freq;
00043     _PWM.period(1/frequency);
00044     
00045 }
00046 void Motor::set_velocity(){
00047     
00048     velocity = (angle-angle_prev)/sample_time;              // differentiate angle TODO: needs filtering!
00049     angle_prev = angle;
00050     
00051 } 
00052 
00053 void Motor::set_angle(){
00054     
00055      int n_pulse= _Encoder.QEI::getPulses();  
00056      
00057       angle =  ((n_pulse)*(360/32))/131;                      // get angle    TODO: Change 32 to 4th value of encoder
00058     cout<<angle<<endl;
00059 }
00060 
00061 void Motor::control_velocity(float vel){
00062         
00063     float control_velocity=vel;
00064     
00065     //float error = velocity - control_velocity;                //set error probably need some kind of PID
00066     
00067     if(angle >= (safety_angle-3) and _direction == !POS) 
00068     {
00069         _PWM = 0;
00070     }
00071     else if(angle <= -(safety_angle-3) and _direction == !NEG) 
00072     {
00073         _PWM = 0;
00074     }
00075     else
00076     {
00077         _PWM = control_velocity ; 
00078     }    
00079 
00080    
00081 }
00082 
00083 float Motor::get_velocity(){
00084     
00085     return velocity;
00086 }
00087 
00088 float Motor::get_period(){
00089     
00090     return 1/frequency;
00091     
00092 }