Diff: Motor.cpp

Revision:

4:5ae9f8b3a16f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.cpp	Tue Nov 24 03:56:22 2015 +0000
@@ -0,0 +1,65 @@
+#include "Motor.h"
+
+Motor::Motor(
+    PinName pwm_pin, PinName min_1_pin, PinName min_2_pin, float pwm_period,
+    PinName current_pin,
+    PinName enc_a_pin, PinName enc_b_pin, 
+    float pid_cur_p_gain, float pid_cur_d_gain, float pid_cur_i_gain) : 
+    hbridge_(pwm_pin, min_1_pin, min_2_pin, pwm_period), 
+    current_sense_(current_pin), 
+    encoder_(enc_a_pin, enc_b_pin), 
+    current_controller_(pid_cur_p_gain, pid_cur_d_gain, pid_cur_i_gain) {
+    
+    timer_.start();
+}
+
+void Motor::update(void) {
+    this->velocity = this->get_velocity();
+    this->current = this->get_current();
+    current_controller_.set_command(this->command);
+    this->output = current_controller_.command_torque(this->current);
+    this->torque = current_controller_.current_torque;
+    this->hbridge_.set_output(this->output);
+}
+
+float Motor::get_position(void) {
+    return encoder_.get_position();
+}
+
+// Implicitly updates position
+float Motor::get_velocity(void) {
+    float old_position = this->position;
+    this->position = this->get_position();
+    float velocity = (this->position - old_position)/timer_.read();
+    timer_.reset();
+    return velocity;
+}
+
+float Motor::get_current(void) {
+    return current_sense_.get_current();
+}
+
+void Motor::set_command(float command) {
+    this->command = command;
+}    
+
+//Useful for testing peak torque.
+//pulse_time = motor on time.  pulse_interval = motor off time
+/*float Motor::get_pulse_current(float pulse_time, float pulse_interval) {   
+    timer.start();
+    int count = 0;
+    float sum = 0.0;
+    this->write(1.0);
+    while (timer.read() < pulse_time){
+        sum += current_pin_.read();                    //Prints current draw to PC
+        count++;
+        //printf("%F", (Current.read())*3.3*4/.525);  //Arithmetic converts the analog input to Amps
+        //printf("\n");                               //(0-3.3V input) * (voltage divider muiltiplier) / (.525 V/A)
+    }
+    timer.stop();
+    timer.reset();
+    wait(pulse_time);
+    this->write(0.0);
+    wait(pulse_interval);
+    return sum/count;
+}*/
\ No newline at end of file