LI SU
/
Motor_Gripper_Control_FSR
Motor_Gripper_Control_FSR
Revision 0:43250c03ce7b, committed 2018-08-22
- Comitter:
- noname001
- Date:
- Wed Aug 22 07:59:49 2018 +0000
- Commit message:
- Gripper_FSR;
Changed in this revision
diff -r 000000000000 -r 43250c03ce7b PID.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Wed Aug 22 07:59:49 2018 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/LDSC_Robotics_TAs/code/PID/#4df4895863cd
diff -r 000000000000 -r 43250c03ce7b main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Aug 22 07:59:49 2018 +0000 @@ -0,0 +1,238 @@ +#include "mbed.h" +#include "PID.h" + +PwmOut mypwm(D7); +AnalogIn analog_value_left(A3); +AnalogIn analog_value_right(A0); + +Ticker timer1; +Serial pc(SERIAL_TX, SERIAL_RX); +Serial uart_1(D10, D2); + +int count = 1000; +int init_PW = 1000; + + +float T = 0.002; +float meas = 0; +float meas_old = 0; +float meas_f = 0; +float meas_right = 0; +float meas_right_old = 0; +float meas_right_f = 0; +float meas_min = 0; +float T_read = 0; +float Td = 200.0; +float err = 0; +float err_old = 0; +float derivative = 0; +float dt = 0.002; + +float PI_out = 0; +float PIDout = 0; +float Kp = 1.0; +float Kd = 1.0; +float Scale = 0.02; + +int count_LR = 0; + + +int display[6] = {0,0,0,0,0,0}; +char num[14] = {254,255,0,0,0,0,0,0,0,0,0,0,0,0}; // char num[0] , num[1] : 2 start byte + + +float lpf(float input, float output_old, float frequency); + +void init_TIMER(void); +void timer1_interrupt(void); +void init_uart(void); +void separate(void); +void uart_send(void); + +PID servo_pid(1.5, 1.0, 0.1, dt); + +int main() { + pc.printf("GOGOGO\n"); + + pc.baud(9600); + uart_1.baud(115200); + mypwm.period_ms(20); + mypwm.pulsewidth_us( init_PW ); + + init_TIMER(); + + while(1) { + + } +} + +void init_TIMER(){ + + timer1.attach_us(&timer1_interrupt, 2000.0); // 10ms +} + +void timer1_interrupt(){ + /* + if(count_LR == 0) + { + count_LR = 1; + + meas = analog_value_left.read(); // Converts and read the analog input value (value from 0.0 to 1.0) + meas = meas * 1000; // Change the value to be in the 0 to 3300 range + + meas_f = lpf(meas,meas_old,20); + meas_old = meas_f; + + + } + else + { + count_LR = 0; + + meas_right = analog_value_right.read(); // Converts and read the analog input value (value from 0.0 to 1.0) + meas_right = meas_right * 1000; // Change the value to be in the 0 to 3300 range + + meas_right_f = lpf(meas_right,meas_right_old,20); + meas_right_old = meas_right_f; + } + */ + meas_right = analog_value_right.read(); // Converts and read the analog input value (value from 0.0 to 1.0) + meas_right = meas_right * 1000; // Change the value to be in the 0 to 3300 range + + meas_right_f = lpf(meas_right,meas_right_old,20); + meas_right_old = meas_right_f; + + + meas = analog_value_left.read(); // Converts and read the analog input value (value from 0.0 to 1.0) + meas = meas * 1000; // Change the value to be in the 0 to 3300 range + + meas_f = lpf(meas,meas_old,20); + meas_old = meas_f; + + + if( meas_f < meas_right_f ) + { + meas_min = meas_f; + } + else + { + meas_min = meas_right_f; + } + + err = Td - meas_min; + derivative = ( err - err_old ) / dt; + err_old = err; + + + servo_pid.Compute(Td, meas_f); + PIDout = servo_pid.output; + count = count + int(Scale * PIDout); + + + //count = 2000; + + + + if(count < 1000) //1700 + { + count = 1000; //1850 + } + else if(count > 1370) //1700 + { + count = 1370; //1850 + } + + + uart_send(); + +/* + count = count - 20; + + + if(count < 1460) + { + count = 1460; //1850 + } + else if(count > 2000) + { + count = 2000; //1850 + } + */ + + mypwm.pulsewidth_us(count); + printf("measure = %.0f mV %.0f mV count = %d \n", meas_f,meas_right_f, count); + + //pc.printf("pwm set to %d %%\n", count); + + + + +} + +float lpf(float input, float output_old, float frequency) +{ + float output = 0; + + output = (output_old + frequency*T*input) / (1 + frequency*T); + + return output; +} + +int i = 0; +void uart_send(void) +{ + // uart send data + display[0] = meas_f; + display[1] = meas_right_f; + display[2] = 100*PIDout; + display[3] = 100*Kp * err; + display[4] = 100*PI_out; + display[5] = count; + + //wait(0.2); + separate(); + + int j = 0; + int k = 1; + while(k) + { + if(uart_1.writeable()) + { + uart_1.putc(num[i]); + i++; + j++; + } + + if(j>1) // send 2 bytes + { + k = 0; + j = 0; + } + } + + if(i>13) + i = 0; +} + +void init_uart() +{ + uart_1.baud(115200); // 設定baudrate +} + +void separate(void) +{ + num[2] = display[0] >> 8; // HSB(8bit)資料存入陣列 + num[3] = display[0] & 0x00ff; // LSB(8bit)資料存入陣列 + num[4] = display[1] >> 8; + num[5] = display[1] & 0x00ff; + num[6] = display[2] >> 8; + num[7] = display[2] & 0x00ff; + num[8] = display[3] >> 8; + num[9] = display[3] & 0x00ff; + num[10] = display[4] >> 8; + num[11] = display[4] & 0x00ff; + num[12] = display[5] >> 8; + num[13] = display[5] & 0x00ff; + + +}
diff -r 000000000000 -r 43250c03ce7b mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Aug 22 07:59:49 2018 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed/builds/64910690c574 \ No newline at end of file