test not done
Dependencies: mbed QEI QEI_hw SDFileSystem
Revision 1:a7c5a3920ba8, committed 2019-10-30
- Comitter:
- chanaka_madhusanka
- Date:
- Wed Oct 30 06:52:53 2019 +0000
- Parent:
- 0:0205108c2c99
- Commit message:
- 30.10.2019 system now working
Changed in this revision
SDFileSystem.lib | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 0205108c2c99 -r a7c5a3920ba8 SDFileSystem.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SDFileSystem.lib Wed Oct 30 06:52:53 2019 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/SDFileSystem/#8db0d3b02cec
diff -r 0205108c2c99 -r a7c5a3920ba8 main.cpp --- a/main.cpp Thu Aug 08 08:12:52 2019 +0000 +++ b/main.cpp Wed Oct 30 06:52:53 2019 +0000 @@ -1,53 +1,333 @@ #include "mbed.h" +#include "SDFileSystem.h" +#include "QEI.h" +#include <math.h> + +SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board p5 - mosi, p6-miso, p7 - sck, p8 - cs +Serial pc (USBTX,USBRX); +QEI wheel (p29, p30, NC, 2000, QEI::X4_ENCODING); //to get encoder position + +//int i= 2; +int value = 0; + +void PID(); +void motor(); +void DOB(double derivative_position); +void RTOB(double derivative_position); +void sendDAC(int value); +void file_writer(double input, double torque); + +double dt=0.0002,t=0; //time +double Kp=0,Ki=0,Kd=0; //pid +double torque_rtob,rtob_filter_input, rtob_filter_output, rtob_filter_integral = 0, rtob_additional_torque; // +double torque_dob, dob_filter_input, dob_filter_output, dob_filter_integral = 0; +double pre_val=0, g_dis=150, error_val=5, k=0.000000085, torque_command, pre_position=0,derivative_val, J=0.462, Jn=0.462, Kt1=25.0, Kt2=25.0, pid_val, motor_voltage, motor_input, motor_torque, motor_current, load_torque, integral1, theta_dot = 0, theta; + + +//DAC 712 +DigitalOut dac_a0(p13); +DigitalOut dac_a1(p12); +DigitalOut dac_wr(p9); +DigitalOut dac_clr(p10); +DigitalOut dac_d15(p15); +DigitalOut dac_d14(p16); +DigitalOut dac_d13(p17); +DigitalOut dac_d12(p18); +DigitalOut dac_d11(p19); +DigitalOut dac_d10(p20); +DigitalOut dac_d9(p21); +DigitalOut dac_d8(p22); +DigitalOut dac_d7(p23); +DigitalOut dac_d6(p24); +DigitalOut dac_d5(p25); +DigitalOut dac_d4(p26); +DigitalOut dac_d3(p27); +DigitalOut dac_d2(p28); +DigitalOut dac_d1(p11); +DigitalOut dac_d0(p14); -#include "qeihw.h" - -DigitalOut led1 (LED1); -DigitalOut led3 (LED3); +int main(){ + + float val = 32767/10; + int sendval; + double position; + double x; + + //while(1){ + //reverse loop + dac_a0 = 0; + dac_a1 = 1; + dac_clr = 1; + sendval = int(val * 0.06*(0.9)); //for maximum torque = 46.75 voltage = 2.805 >>> 0 is not zero. Eventhough "0.9" is a positive it gives negative direction movement for "3" it gives positive movement + sendDAC(sendval); + dac_a0 = 1; + dac_a1 = 0; + wait(0.1); //to change the back velocity + + dac_a0 = 0; + dac_a1 = 1; + dac_clr = 1; + sendval = int(val * 0.06*(1.4)); //for maximum torque = 46.75 voltage = 2.805 >>> 0 is not zero. Eventhough "0.9" is a positive it gives negative direction movement for "3" it gives positive movement + sendDAC(sendval); + dac_a0 = 1; + dac_a1 = 0; + wait(3); //to change the back velocity + + //forward loop + dac_a0 = 0; + dac_a1 = 1; + sendval = int(val * 0.06*(6)); + sendDAC(sendval); + dac_a0 = 1; + dac_a1 = 0; + wait(4); + + + position = 0; + x = -wheel.getPulses()/200.0; + + double torque; + torque = 25*0.6*0.4*(0.0284*(15)+0.1436)/1.2796; //maximum percentage is 40... range 0-40 + + while(-0.01>error_val | error_val>0.01) { //minimize the error value between reation torque and torque + + pc.printf("%lf\t %lf\t %lf\r\n",pid_val,torque_rtob,error_val); + + position = (-wheel.getPulses()/200.0)-x; //////////////// + + double derivative_position = (position-pre_position)/dt; + pre_position = position; + + error_val = torque - torque_rtob; + + PID(); + + //DOB(derivative_position); + + RTOB(derivative_position); + + motor_voltage = ((pid_val*J)/25)*(1.2796/(0.6*0.4)); + + //motor_input = motor_voltage + ((torque_dob/25)*(1.2796/(0.6*0.4))); //sendVal=motor_input + motor_input = motor_voltage; + + dac_a0 = 0; + dac_a1 = 1; + sendval = motor_input; + sendDAC(sendval); + dac_a0 = 1; + dac_a1 = 0; + + + + //fprintf (fp3, "%f %f %f\n",t,torque_rtob,load_torque); + + t+=0.0002; + } + + if((wheel.getPulses()/200.0)<6){ + //file_writer(position,torque_rtob); + } + pc.printf("Pulses is: %lf\t %lf\r\n",position,torque_rtob); + -int pulses = 0; -int32_t temp=0; -Serial pc(USBTX, USBRX); -//Use X4 encoding. -//QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING); -//Use X2 encoding by default. -//QEI wheel (p13, p14, NC, 2000); -QEIHW wheel(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_4X, QEI_INVINX_NONE ); - -Ticker tick; - -void display() -{ - pc.printf("Pulses is: %i\n", temp ); - //pc.printf(" State : %i\n", wheel.GetPosition()); + //for(double i = 1 ; i<=20 ; i+=0.2){ //to increase torque +// +// double torque; +// +// if(i<10.1){ +// +// torque = 25*0.6*0.4*(0.0284*(6+i)+0.1436)/1.2796; //maximum percentage is 40... range 0-40 +// +// while(-0.1>error_val | error_val>0.1) { //minimize the error value between reation torque and torque +// pc.printf("%lf\t %lf\t %lf\r\n",torque,torque_rtob,position); +// +// position = (-wheel.getPulses()/200.0)-x; //////////////// +// +// double derivative_position = (position-pre_position)/dt; +// pre_position = position; +// +// error_val = torque - torque_rtob; +// +// PID(); +// +// motor_voltage = ((pid_val*J)/25)*(1.2796/0.6*0.4); +// +// motor_input = motor_voltage + ((torque_dob/25)*(1.2796/0.6*0.4)); //sendVal=motor_input +// +// dac_a0 = 0; +// dac_a1 = 1; +// sendval = motor_input; +// sendDAC(sendval); +// dac_a0 = 1; +// dac_a1 = 0; +// +// DOB(derivative_position); +// RTOB(derivative_position); +// +// //fprintf (fp3, "%f %f %f\n",t,torque_rtob,load_torque); +// +// t+=0.0002; +// } +// +// if((wheel.getPulses()/200.0)<6){ +// //file_writer(position,torque_rtob); +// } +// pc.printf("Pulses is: %lf\t %lf\r\n",position,torque_rtob); +// +// if(i>9.9){ +// for(int j=1; j<500 ;j++){ +// pc.printf("Wait position : %lf\t %lf\r\n",-(wheel.getPulses()/200.0)-x,torque); +// } +// } +// }else if(i>=10.1){ +// +// dac_a0 = 0; +// dac_a1 = 1; +// sendval = int(val * 0.06*(6+20-i)); +// sendDAC(sendval); +// dac_a0 = 1; +// dac_a1 = 0; +// torque = 25*0.6*0.4*(0.0284*(6+20-i)+0.1436)/1.2796 +// position = (-wheel.getPulses()/200.0)-x; +// +// if((wheel.getPulses()/200.0)<6){ +// //file_writer(position,torque); +// } +// pc.printf("Pulses is: %lf\t %lf\r\n",position,torque); +// } +// wait(0.05); +// } + //} } -int main() { - +//set voltages to dac +void sendDAC(int value){ + int remainder = 0; + dac_clr = 0; + wait_us(120); + dac_clr = 1; + //send output dac + dac_wr = 1; + if(value >= 0){ + if(value >= 32767){ + dac_d15 = 0; dac_d14= 1; dac_d13 = 1; dac_d12 = 1; dac_d11 = 1; dac_d10 =1; dac_d9 = 1; dac_d8 = 1; dac_d7 = 1; dac_d6 = 1; dac_d5 = 1; dac_d4 = 1; dac_d3 = 1; dac_d2 = 1; dac_d1 =1; dac_d0 = 1; + } + else { + dac_d15 = 0; + dac_d14 = value / 16384 ; + remainder = value - dac_d14.read() * 16384; + dac_d13 = remainder / 8192; + remainder = remainder - dac_d13.read() * 8192; + dac_d12 = remainder / 4096; + remainder = remainder - dac_d12.read() * 4096; + dac_d11 = remainder / 2048; + remainder = remainder - dac_d11.read() * 2048; + dac_d10 = remainder / 1024; + remainder = remainder - dac_d10.read() * 1024; + dac_d9 = remainder / 512; + remainder = remainder - dac_d9.read() * 512; + dac_d8 = remainder / 256; + remainder = remainder - dac_d8.read() * 256; + dac_d7 = remainder / 128; + remainder = remainder - dac_d7.read() * 128; + dac_d6 = remainder / 64; + remainder = remainder - dac_d6.read() * 64; + dac_d5 = remainder / 32; + remainder = remainder - dac_d5.read() * 32; + dac_d4 = remainder / 16; + remainder = remainder - dac_d4.read() * 16; + dac_d3 = remainder / 8; + remainder = remainder - dac_d3.read() * 8; + dac_d2 = remainder / 4; + remainder = remainder - dac_d2.read() * 4; + dac_d1 = remainder / 2; + remainder = remainder - dac_d1.read() * 2; + dac_d0 = remainder ; + } + } + if (value < 0){ + if(value <= -32768){ + dac_d15 = 1; dac_d14= 0; dac_d13 = 0; dac_d12 = 0; dac_d11 = 0; dac_d10 =0; dac_d9 = 0; dac_d8 = 0; dac_d7 = 0; dac_d6 = 0; dac_d5 = 0; dac_d4 = 0; dac_d3 = 0; dac_d2 = 0; dac_d1 =0; dac_d0 = 0; + } + else{ + dac_d15 = 1; + dac_d14 = ( 32768 + value) / 16384 ; + remainder = ( 32768 + value) - dac_d14.read() * 16384; + dac_d13 = remainder / 8192; + remainder = remainder - dac_d13.read() * 8192; + dac_d12 = remainder / 4096; + remainder = remainder - dac_d12.read() * 4096; + dac_d11 = remainder / 2048; + remainder = remainder - dac_d11.read() * 2048; + dac_d10 = remainder / 1024; + remainder = remainder - dac_d10.read() * 1024; + dac_d9 = remainder / 512; + remainder = remainder - dac_d9.read() * 512; + dac_d8 = remainder / 256; + remainder = remainder - dac_d8.read() * 256; + dac_d7 = remainder / 128; + remainder = remainder - dac_d7.read() * 128; + dac_d6 = remainder / 64; + remainder = remainder - dac_d6.read() * 64; + dac_d5 = remainder / 32; + remainder = remainder - dac_d5.read() * 32; + dac_d4 = remainder / 16; + remainder = remainder - dac_d4.read() * 16; + dac_d3 = remainder / 8; + remainder = remainder - dac_d3.read() * 8; + dac_d2 = remainder / 4; + remainder = remainder - dac_d2.read() * 4; + dac_d1 = remainder / 2 ; + remainder = remainder - dac_d1.read() * 2; + dac_d0 = remainder ; + } + } + dac_wr = 0; +} + +void file_writer(double position,double torque){ + mkdir("/sd/mydir", 0777); + FILE *fp = fopen("/sd/mydir/sdtesthys.txt", "a"); + if(fp == NULL){ + error("Could not open file for write\n"); + } + fprintf(fp,"%lf\t %lf\t \r\n",position,torque); + fclose(fp); +} + +//PID controller +void PID(){ + integral1 = integral1 + error_val*dt; + derivative_val = (error_val - pre_val)/dt; + pre_val = error_val; + pid_val = (0*error_val)+(0*integral1)+(0*derivative_val); +} + +//disturbance observer +void DOB(double derivative_position){ //disturbance observer + dob_filter_input = (derivative_position * g_dis * Jn) + (motor_input * Kt2 * (0.6*0.4/1.2796)); //g = g_dis / J_n / filter_input_dob = dob_filter_input + dob_filter_integral = dob_filter_integral + ((dob_filter_input - dob_filter_output) * dt); //filter + dob_filter_output = g_dis * dob_filter_integral; + torque_dob = dob_filter_output - (Jn * g_dis); +} + +//reaction torque observer +void RTOB(double derivative_position){ + rtob_additional_torque = 1.10745; //ask sir + rtob_filter_input = (derivative_position * g_dis * Jn) + (motor_input * Kt2 * (0.6*0.4/1.2796)) - rtob_additional_torque; + rtob_filter_integral = rtob_filter_integral + ((rtob_filter_input - rtob_filter_output) * dt); //fil + rtob_filter_output = g_dis * rtob_filter_integral; + torque_rtob = rtob_filter_output - (derivative_position * g_dis * Jn); +} + +//Threading code +void thread_2(void const *argument){ while(1){ - wait(0.1); - wheel.SetDigiFilter(480UL); - wheel.SetMaxPosition(0xFFFFFFFF); - temp = wheel.GetPosition(); - // pulses = wheel.(); - //tick.attach (&display , 1); - - display(); - if (temp >20000){ - led1 = 1; - led3 = 1; - } - else if (temp >10000){ - led1 = 1; - led3 = 0; - } - else if (temp >5000){ - led1 = 0; - led3 = 1; - } - else{ - led1 = 0; - led3 = 0; + //pc.printf("%i\t %f %f\t \r\n)",count,dx_res,I_msrd); + //file_writer(); } } -} \ No newline at end of file + + +