Motor.cpp

00001 #include "Motor.h"
00002 
00003 Motor::Motor(
00004     PinName pwm_pin, PinName min_1_pin, PinName min_2_pin, float pwm_period,
00005     PinName current_pin,
00006     PinName enc_a_pin, PinName enc_b_pin, 
00007     float pid_cur_p_gain, float pid_cur_d_gain, float pid_cur_i_gain) : 
00008     hbridge_(pwm_pin, min_1_pin, min_2_pin, pwm_period), 
00009     current_sense_(current_pin), 
00010     encoder_(enc_a_pin, enc_b_pin), 
00011     current_controller_(pid_cur_p_gain, pid_cur_d_gain, pid_cur_i_gain) {
00012     
00013     timer_.start();
00014 }
00015 
00016 void Motor::update(void) {
00017     this->velocity = this->get_velocity();
00018     this->current = this->get_current();
00019     current_controller_.set_command(this->command);
00020     this->output = current_controller_.command_torque(this->current);
00021     this->torque = current_controller_.current_torque;
00022     this->hbridge_.set_output(this->output);
00023 }
00024 
00025 float Motor::get_position(void) {
00026     return encoder_.get_position();
00027 }
00028 
00029 // Implicitly updates position
00030 float Motor::get_velocity(void) {
00031     float old_position = this->position;
00032     this->position = this->get_position();
00033     float velocity = (this->position - old_position)/timer_.read();
00034     timer_.reset();
00035     return velocity;
00036 }
00037 
00038 float Motor::get_current(void) {
00039     return current_sense_.get_current();
00040 }
00041 
00042 void Motor::set_command(float command) {
00043     this->command = command;
00044 }    
00045 
00046 //Useful for testing peak torque.
00047 //pulse_time = motor on time.  pulse_interval = motor off time
00048 /*float Motor::get_pulse_current(float pulse_time, float pulse_interval) {   
00049     timer.start();
00050     int count = 0;
00051     float sum = 0.0;
00052     this->write(1.0);
00053     while (timer.read() < pulse_time){
00054         sum += current_pin_.read();                    //Prints current draw to PC
00055         count++;
00056         //printf("%F", (Current.read())*3.3*4/.525);  //Arithmetic converts the analog input to Amps
00057         //printf("\n");                               //(0-3.3V input) * (voltage divider muiltiplier) / (.525 V/A)
00058     }
00059     timer.stop();
00060     timer.reset();
00061     wait(pulse_time);
00062     this->write(0.0);
00063     wait(pulse_interval);
00064     return sum/count;
00065 }*/