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
- Committer:
- Alex_Kyrl
- Date:
- 2017-10-30
- Revision:
- 12:69a9cf74583e
- Parent:
- 11:dd1976534a03
- Child:
- 13:559f8946f16d
File content as of revision 12:69a9cf74583e:
#include "Motor.h" Motor::Motor() : _Encoder(D13,D12,NC,32), _direction(D7) , _PWM(D6) { _PWM=0; _Encoder.QEI::reset(); frequency=50000; set_period(frequency); angle=90; safety_angle = 120; //safety angle in DEGREES _direction= 1; low_PWM = 0.6; } Motor::Motor(PinName a , PinName b, PinName direction , PinName PWM , float freq , float safe , float low ) : _Encoder(a,b,NC,32), _direction(direction) , _PWM(PWM) { _PWM=0; _Encoder.QEI::reset(); frequency=freq; set_period(frequency); angle= 90; safety_angle = safe; _direction= 1; low_PWM= low; } void Motor::set_period(float freq){ frequency=freq; _PWM.period(1/frequency); } float Motor::set_angle(){ int n_pulse= _Encoder.QEI::getPulses(); angle = 90 + ((n_pulse)*(360/32))/131; // get angle (131 is the gear ratio of the motor in order to work in degrees of the actual arm) return angle; } double Motor::Control_angle(float ang){ float control_angle=ang; //float step = 360/(32*131); //control_angle = control_angle - fmodf(control_angle, step) ; //changle control angle to posible angles of the sensor if(control_angle > safety_angle+90) control_angle = safety_angle+90; if(control_angle < -safety_angle+90) control_angle = -safety_angle+90; double error = angle - control_angle; //set error probably need some kind of PID if(error >= 0) _direction = 1; else _direction = 0; if( 0.1 < fabs(error) < low_PWM) _PWM = low_PWM; else if(fabs(error) >= 1 ) _PWM = 1; else if (fabs(error) < 0.1) _PWM = 0; else _PWM = fabs(error); return error; }