jiexuan fan
a
Diff: main.cpp
- Revision:
- 5:f82e86e63928
- Parent:
- 4:4e7b392ed0aa
--- a/main.cpp Wed Mar 18 17:17:58 2015 +0000
+++ b/main.cpp Mon Dec 24 03:44:24 2018 +0000
@@ -1,37 +1,33 @@
-//Ben Katz, 2013
-
#include "QEI.h"
#include "PID.h"
+#include "mbed.h"
+#include "math.h"
+
#define pi 3.14159265358979323846
-Serial pc(USBTX, USBRX);
+Serial PC(USBTX, USBRX);
-AnalogIn p_pot (p15);
-AnalogIn d_pot (p16);
-AnalogIn a_pot (p17);
+AnalogIn Current(p17); //Current Connect to pin CS
+PwmOut pwm(p22);
+DigitalOut INA(p19);
+DigitalOut INB(p20);
-PwmOut Motor1(p21); //Ouput pin to h-bridge 1
-PwmOut Motor2 (p22); //Output pin to h-bridge 2
-AnalogIn Current (p20); //Current sense pin
+Ticker position_ticker;
+
int steps_per_rev = 1200; //Encoder CPR * gearbox ratio
Timer timer;
// Setup parameters
-float volt2amp = 3.3 / .525; //(0-3.3V input) * (voltage divider muiltiplier) / (.525 V/A)
+float volt2amp = 3.3 / .140; //(0-3.3V input) * (voltage divider muiltiplier) / (.140 V/A)
float PWM_frequency = 20000.0; // MC33926 h-bridge limited 20 kHz (.00005 seconds)
//Use X4 encoding.
-QEI wheel(p13, p14, NC, steps_per_rev, QEI::X4_ENCODING); //(encoder channel 1, encoder channel 2, index (n/a here), counts per revolution, mode (X4, X2))
-
+QEI wheel(p15, p16, NC, steps_per_rev, QEI::X4_ENCODING); //(encoder channel 1, encoder channel 2, index (n/a here), counts per revolution, mode (X4, X2))
-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
{
@@ -43,71 +39,111 @@
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);
+ INA=1;
+ INB=0;
+ pwm.write(1);
} else if (signal > 0) {
- Motor1.write(signal);
- Motor2.write(0);
+ INA=0;
+ INB=1;
+ pwm.write(1);
} else {
- Motor1.write(0);
- Motor2.write(0);
+ INA=0;
+ INB=0;
+ pwm.write(0);
}
}
-
-
-void pulse_motor(float pulse_time, float pulse_interval) //Usefull for testing peak torque.
- //pulse_time = motor on time. pulse_interval = motor off time
-{
- timer.start();
+
- while (timer.read() < pulse_time){
- Motor1.write(1);
- Motor2.write(0);
- //Prints current draw to PC
- printf("%F", (Current.read())*volt2amp); //Arithmetic converts the analog input to Amps
- printf("\n");
- }
-
- timer.stop();
- timer.reset();
-
- wait(pulse_time);
-
- Motor1.write(0);
- Motor2.write(0);
-
- wait(pulse_interval);
- }
-
-
+void initialize()
+{
+ INA=0;
+ INB=0;
+ pwm.write(0);
+}
+void forward()
+{
+ INA.write(1);
+ INB.write(0);
+ pwm.write(1);
+}
+void back()
+{
+ INA.write(0);
+ INB.write(1);
+ pwm.write(1);
+}
+void begin()
+{
+ INA.write(1);
+ INB.write(0);
+ pwm.write(0.05);
+}
+int get_position(int position)
+{
+ if (position > 40){
+ position = 40;
+ return position;
+ }
+ else if (position< 1){
+ position = 1;
+ return position;
+ }
+ else
+ {
+ return position;
+ }
+}
int main()
{
-
- Motor1.period(1 / PWM_frequency); //Set PWM frequency. MC33926 h-bridge limited 20 kHz (.00005 seconds)
- Motor2.period(1 / PWM_frequency);
-
- float desired_position = 40;
- float torque = 0.1;
- float position_P_gain = 0.1;
- float position_D_gain = 3;
- float position_I_gain = 0;
- float torque_P_gain = 0.0007;
- float torque_I_gain = 0.0000001;
-
-
- PIDController controller(desired_position, torque, position_P_gain, position_D_gain, position_I_gain, torque_P_gain, torque_I_gain);
- //timer.start();
-
+ int position;
+ wait(5);
+ printf("Iiitializing.....\n\r");
+
+ initialize();
+ pwm.period(1/ PWM_frequency); //Set PWM frequency. limited 20 kHz (.00005 seconds)
+
wait(1.5);
- printf("%F", pulsesToDegrees(wheel.getPulses()));
- printf("\n\r");
+ printf("Begining\n\r");
+
+
+ //position_ticker.attach(&update_position, 0.5);
+ begin();
+ wait(2);
+ int direction;
+
+ while(1){
- while(1){
- signal_to_hbridge(controller.command_position_tm());
- printf("%F", pulsesToDegrees(wheel.getPulses()));
+ int current_position=pulsesToDegrees(wheel.getPulses());
+
+ int position=get_position(current_position);
+
+ if(position==1||position==0)
+ {
+ direction=1;
+ }
+ else if(position==40)
+ {
+ direction=-1;
+ }
+
+
+ float command = sin(direction*position*pi/40 );
+
+
+ if(command>0)
+ { forward();}
+ else if(command<0)
+ { back();}
+
+
+
+ printf("position:%d\n",position);
+ printf("%f\n",command);
+
+ //signal_to_hbridge(controller.command_position());
}
}
@@ -148,90 +184,17 @@
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);
-
+ float current_Position = pulsesToDegrees(wheel.getPulses()); //Change pulsesToDegrees() to pulsesToRadians() if you would prefer to use radians
+ int currentPosition=(int)current_Position;
+ this->error = (currentPosition - desired_position);
+ this->integral_error += this->error;
+ if (this->integral_error > 1.0)
+ this->integral_error = 1.0;
+ else if (this->integral_error < -1.0)
+ this->integral_error = -1.0;
+ 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;
-
-}
-
-
-float PIDController::command_position_tm(void){ //Torque Mode position control
- wait(.0003);
-
- 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();
- }
-
-
- return this->command_torque();
-
-
}
-
-
-float PIDController::command_torque(void){
- wait(.0004);
- float current_torque;
-
- if (this->direction != 0){
- current_torque = this->direction*(Current.read() * volt2amp);
- }
- else{
- current_torque = (Current.read() * volt2amp);
- }
- 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;
-}