jiexuan fan
mit
Diff: main.cpp
- Revision:
- 4:5ae9f8b3a16f
- Parent:
- 3:cae0b305d54c
- Child:
- 5:e90c8b57811c
--- a/main.cpp Fri Jul 12 00:13:37 2013 +0000
+++ b/main.cpp Tue Nov 24 03:56:22 2015 +0000
@@ -1,227 +1,104 @@
//Ben Katz, 2013
-
-#include "QEI.h"
+//Austin Buchan, 2015
+#include "mbed.h"
+#include "Motor.h"
#include "PID.h"
-#define pi 3.14159265358979323846
-
-
-Serial pc(USBTX, USBRX);
-
-//Use X4 encoding.
-QEI wheel(p13, p14, NC, 1200, QEI::X4_ENCODING); //(encoder channel 1, encoder channel 2, index (n/a here), counts per revolution, mode (X4, X2))
-
-AnalogIn p_pot (p15);
-AnalogIn d_pot (p16);
-AnalogIn a_pot (p17);
-
-PwmOut Motor1(p21); //Ouput pin to h-bridge 1
-PwmOut Motor2 (p22); //Output pin to h-bridge 2
-AnalogIn Current (p20); //Current sense pin
-
-int steps_per_rev = 1200; //Encoder CPR * gearbox ratio
-Timer timer;
-
-
+#define PWM_PIN PTB13
+#define MIN_1_PIN PTA0
+#define MIN_2_PIN PTA8
+#define ENC_A_PIN PTB6
+#define ENC_B_PIN PTB7
+#define CURRENT_PIN PTA9
-float pulsesToRadians(float pulses) //Encoder steps to revolutions of output shaft in radians
-{
- return ((pulses/steps_per_rev)*(2*pi));
-}
-
-float pulsesToDegrees(float pulses) //Encoder steps to revolutions of output shaft in degrees
-{
-
- return ((pulses/steps_per_rev)*360);
-}
-
+#define UART_RX_PIN PTB1
+#define UART_TX_PIN PTB2
-void signal_to_hbridge( float signal) //Input of -1 means full reverse, 1 means full forward
-{ //An input of magnitude > 1 gets reduced to 1 (or -1) by the pwm.write() function
- if (signal < 0) {
- Motor1.write(0);
- Motor2.write(signal * -1);
- } else if (signal > 0) {
- Motor1.write(signal);
- Motor2.write(0);
- } else {
- Motor1.write(0);
- Motor2.write(0);
- }
-}
+#define PWM_PERIOD 0.001
+#define PID_CUR_PERIOD 0.001
+#define PID_POS_PERIOD 0.005
+
+#define PID_POS_P_GAIN 0.5
+#define PID_POS_D_GAIN 10.0
+#define PID_POS_I_GAIN 0.1
-
-
-void pulse_motor(float pulse_time, float pulse_interval) //Usefull for testing peak torque.
- //pulse_time = motor on time. pulse_interval = motor off time
-{
+#define PID_CUR_P_GAIN 0.5
+#define PID_CUR_D_GAIN 0.0
+//#define PID_CUR_I_GAIN 0.001
+#define PID_CUR_I_GAIN 0.01
- timer.start();
- while (timer.read() < pulse_time){
- Motor1.write(1);
- Motor2.write(0);
- //Prints current draw to PC
- 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);
- Motor1.write(0);
- Motor2.write(0);
- wait(pulse_interval);
-}
-
+Serial pc(UART_TX_PIN, UART_RX_PIN);
-
-int main()
-{
+Motor motor(
+ PWM_PIN, MIN_1_PIN, MIN_2_PIN, PWM_PERIOD,
+ CURRENT_PIN,
+ ENC_A_PIN, ENC_B_PIN,
+ PID_CUR_P_GAIN, PID_CUR_D_GAIN, PID_CUR_I_GAIN
+);
- Motor1.period(.00005); //Set PWM frequency. MC33926 h-bridge limited 20 kHz (.00005 seconds)
- Motor2.period(.00005);
- PIDController controller(40, .1, 0.1, 3, 0, 0.0007, 0.0000001); //(desired position, torque, position P gain, position D gain, position I gain, torque P gain, torque I gain)
- //timer.start();
- //float currentSum = 0;
- //float angleSum = 0;
-
- wait(.7);
-
- wait(.5);
- printf("%F", pulsesToDegrees(wheel.getPulses()));
- printf("\n\r");
- while(1){
- signal_to_hbridge(controller.command_position_tm());
- //printf("%F", pulsesToDegrees(wheel.getPulses()));
- }
-
-
-
+//HBridge hbridge(PWM_PIN, MIN_1_PIN, MIN_2_PIN, PWM_PERIOD);
+//CurrentSense current_sense(CURRENT_PIN);
+//Encoder encoder(ENC_A_PIN, ENC_B_PIN);
+
+PIDController position_controller(
+ PID_POS_P_GAIN, PID_POS_D_GAIN, PID_POS_I_GAIN
+);
+
+Ticker current_ticker;
+
+void update_current(void) {
+ motor.update();
}
-
-
-PIDController::PIDController(float desired_position, float desired_torque, float p_gain_p, float d_gain_p, float i_gain_p, float p_gain_c, float i_gain_c)
-{
- kp_p = p_gain_p;
- kd_p = d_gain_p;
- ki_p = i_gain_p;
- kp_c = p_gain_c;
- ki_c = i_gain_c;
- torque = desired_torque;
- current_position = 0;
- torque_command = 0;
- c_torque = 0;
- error = 0;
- old_error = 0;
- error_sum = 0;
- goal_position = desired_position;
- direction = 0;
- counter = 0;
- timer.start();
- command = 0;
-}
+Ticker position_ticker;
-//voltage mode position control
-float PIDController::command_position(void) //This function is called to set the desired position of the servo
-{
- // wait(.0004); //This delay allows enough time to register a position difference between cycles.
- //Without the delay, velocity is read as 0, so there is no D feedback.
- //The delay can be decreased when adding more servos, as the computation time becomes more significant.
-
- float desired_position = this->goal_position; //All value are stored in the PIDController object created, and can be changed at any time
- float p_gain = this->kp_p;
- float d_gain = this->kd_p;
- float i_gain = this->ki_p;
- if (this->timer.read_us() > 400){
- float currentPosition = pulsesToDegrees(wheel.getPulses()); //Change pulsesToDegrees() to pulsesToRadians() if you would prefer to use radians
-
- this->error = (currentPosition - desired_position);
- this->integral_error += this->error;
-
- this->command = p_gain*(this->error) + d_gain*(this->error - this->old_error) + i_gain*(this->integral_error);
-
- this->old_error = error;
- }
-
- return this->command;
-
+void update_position(void) {
+ motor.set_command(position_controller.command_position(motor.get_position()));
}
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
-float PIDController::command_position_tm(void){ //Torque Mode position control
- //wait(.0003);
- ///*
-
+int main() {
+ printf("Built " __DATE__ " " __TIME__ "\r\n");
+
+ led1 = 1;
+ led2 = 1;
+ led3 = 1;
+
+ wait(.5);
+
+ led2 = 0;
+ printf("Initializing\n\r");
+
+ current_ticker.attach(&update_current, PID_CUR_PERIOD);
+ position_ticker.attach(&update_position, PID_POS_PERIOD);
+
+ int count = 0;
+ float command = 0.0;
- // */
- //this->current_position = pulsesToDegrees(wheel.getPulses()); //Change pulsesToDegrees() to pulsesToRadians() if you would prefer to use radians
- float desired_position = this->goal_position;
- float p_gain = this->kp_p;
- float d_gain = this->kd_p;
- float i_gain = this->ki_p;
- if (this->timer.read_us() > 400){
- this->current_position = pulsesToDegrees(wheel.getPulses()); //Change pulsesToDegrees() to pulsesToRadians() if you would prefer to use radians
- this->old_error = this->error;
- this->error = (this->current_position - desired_position);
- this-> command = p_gain*(this->error) + d_gain*(this->error - this->old_error) + i_gain*(this->integral_error);
- this->integral_error += this->error;
- this->torque_command = command;
- this->timer.reset();
- }
+ led3 = 0;
+ printf("Starting\n\r");
-
-
-
- return this->command_torque();
-
-
-}
-
-
-
-float PIDController::command_torque(void){
- //wait(.0004);
- float current_torque;
-
- if (this->direction != 0){
- current_torque = this->direction*(Current.read()*3.3/.525);
+ while(1){
+ //command = 0.05 * abs((50-count)/10) - 0.1;
+ //motor.set_command(command);
+
+ command = 1.0 * abs((55-count)/10) - 0.25;
+ position_controller.set_command(command);
+
+ led1 = (count % 2);
+
+ //printf("C:%F Cu:%F P:%F\n\r", command, current_sense.get_current(), encoder.get_position());
+ //printf("PC:%F\n\r", position_controller.command);
+ printf("MP:%F MV:%F MCu:%F MCo:%F MO:%F MT:%F P:%F\n\r",
+ motor.position, motor.velocity, motor.current, motor.command, motor.output, motor.torque, position_controller.command
+ );
+
+
+ count = (count + 1) % 100;
+
+ wait(0.1);
}
- else{
- current_torque = (Current.read()*3.3/.525);
- }
- float average = 0;
-
- for (int i = 0; i < 4; i++){
- this->past_currents[i] = this->past_currents[i+1];
- average += past_currents[i];
- }
- average += current_torque;
- average = average/5;
- average = current_torque;
- this->past_currents[4] = current_torque;
-
- this->c_torque = average;
- this->c_error = (this->torque - average);
-
- this->error_sum += this->c_error;
-
- this->torque_command += -1*(this->kp_c*this->c_error + this->ki_c*this->error_sum);
- if (this->torque_command > 0){
- this->direction = -1;
- }
- else if(this->torque_command < 0){
- this->direction = 1;
- }
- else{
- this->direction = 0;
- }
- if (this->torque_command > 1){
- this->torque_command = 1;
- }
- else if (this->torque_command < -1){
- this->torque_command = -1;
- }
-
- return this->torque_command;
-}
+}
\ No newline at end of file