jiexuan fan / ServoController_2

mit

Dependencies:   QEI mbed-src

Motor.cpp@5:e90c8b57811c, 2018-12-24 (annotated)

Committer:
coldplay
Date:
Mon Dec 24 03:47:49 2018 +0000
Revision:
5:e90c8b57811c
Parent:
4:5ae9f8b3a16f 
mit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
abuchan 4:5ae9f8b3a16f 1 #include "Motor.h"
abuchan 4:5ae9f8b3a16f 2
abuchan 4:5ae9f8b3a16f 3 Motor::Motor(
abuchan 4:5ae9f8b3a16f 4 PinName pwm_pin, PinName min_1_pin, PinName min_2_pin, float pwm_period,
abuchan 4:5ae9f8b3a16f 5 PinName current_pin,
abuchan 4:5ae9f8b3a16f 6 PinName enc_a_pin, PinName enc_b_pin,
abuchan 4:5ae9f8b3a16f 7 float pid_cur_p_gain, float pid_cur_d_gain, float pid_cur_i_gain) :
abuchan 4:5ae9f8b3a16f 8 hbridge_(pwm_pin, min_1_pin, min_2_pin, pwm_period),
abuchan 4:5ae9f8b3a16f 9 current_sense_(current_pin),
abuchan 4:5ae9f8b3a16f 10 encoder_(enc_a_pin, enc_b_pin),
abuchan 4:5ae9f8b3a16f 11 current_controller_(pid_cur_p_gain, pid_cur_d_gain, pid_cur_i_gain) {
abuchan 4:5ae9f8b3a16f 12
abuchan 4:5ae9f8b3a16f 13 timer_.start();
abuchan 4:5ae9f8b3a16f 14 }
abuchan 4:5ae9f8b3a16f 15
abuchan 4:5ae9f8b3a16f 16 void Motor::update(void) {
abuchan 4:5ae9f8b3a16f 17 this->velocity = this->get_velocity();
abuchan 4:5ae9f8b3a16f 18 this->current = this->get_current();
abuchan 4:5ae9f8b3a16f 19 current_controller_.set_command(this->command);
abuchan 4:5ae9f8b3a16f 20 this->output = current_controller_.command_torque(this->current);
abuchan 4:5ae9f8b3a16f 21 this->torque = current_controller_.current_torque;
abuchan 4:5ae9f8b3a16f 22 this->hbridge_.set_output(this->output);
abuchan 4:5ae9f8b3a16f 23 }
abuchan 4:5ae9f8b3a16f 24
abuchan 4:5ae9f8b3a16f 25 float Motor::get_position(void) {
abuchan 4:5ae9f8b3a16f 26 return encoder_.get_position();
abuchan 4:5ae9f8b3a16f 27 }
abuchan 4:5ae9f8b3a16f 28
abuchan 4:5ae9f8b3a16f 29 // Implicitly updates position
abuchan 4:5ae9f8b3a16f 30 float Motor::get_velocity(void) {
abuchan 4:5ae9f8b3a16f 31 float old_position = this->position;
abuchan 4:5ae9f8b3a16f 32 this->position = this->get_position();
abuchan 4:5ae9f8b3a16f 33 float velocity = (this->position - old_position)/timer_.read();
abuchan 4:5ae9f8b3a16f 34 timer_.reset();
abuchan 4:5ae9f8b3a16f 35 return velocity;
abuchan 4:5ae9f8b3a16f 36 }
abuchan 4:5ae9f8b3a16f 37
abuchan 4:5ae9f8b3a16f 38 float Motor::get_current(void) {
abuchan 4:5ae9f8b3a16f 39 return current_sense_.get_current();
abuchan 4:5ae9f8b3a16f 40 }
abuchan 4:5ae9f8b3a16f 41
abuchan 4:5ae9f8b3a16f 42 void Motor::set_command(float command) {
abuchan 4:5ae9f8b3a16f 43 this->command = command;
abuchan 4:5ae9f8b3a16f 44 }
abuchan 4:5ae9f8b3a16f 45
abuchan 4:5ae9f8b3a16f 46 //Useful for testing peak torque.
abuchan 4:5ae9f8b3a16f 47 //pulse_time = motor on time. pulse_interval = motor off time
abuchan 4:5ae9f8b3a16f 48 /*float Motor::get_pulse_current(float pulse_time, float pulse_interval) {
abuchan 4:5ae9f8b3a16f 49 timer.start();
abuchan 4:5ae9f8b3a16f 50 int count = 0;
abuchan 4:5ae9f8b3a16f 51 float sum = 0.0;
abuchan 4:5ae9f8b3a16f 52 this->write(1.0);
abuchan 4:5ae9f8b3a16f 53 while (timer.read() < pulse_time){
abuchan 4:5ae9f8b3a16f 54 sum += current_pin_.read(); //Prints current draw to PC
abuchan 4:5ae9f8b3a16f 55 count++;
abuchan 4:5ae9f8b3a16f 56 //printf("%F", (Current.read())*3.3*4/.525); //Arithmetic converts the analog input to Amps
abuchan 4:5ae9f8b3a16f 57 //printf("\n"); //(0-3.3V input) * (voltage divider muiltiplier) / (.525 V/A)
abuchan 4:5ae9f8b3a16f 58 }
abuchan 4:5ae9f8b3a16f 59 timer.stop();
abuchan 4:5ae9f8b3a16f 60 timer.reset();
abuchan 4:5ae9f8b3a16f 61 wait(pulse_time);
abuchan 4:5ae9f8b3a16f 62 this->write(0.0);
abuchan 4:5ae9f8b3a16f 63 wait(pulse_interval);
abuchan 4:5ae9f8b3a16f 64 return sum/count;
abuchan 4:5ae9f8b3a16f 65 }*/