![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
test not done
Dependencies: mbed QEI QEI_hw SDFileSystem
main.cpp@1:a7c5a3920ba8, 2019-10-30 (annotated)
- Committer:
- chanaka_madhusanka
- Date:
- Wed Oct 30 06:52:53 2019 +0000
- Revision:
- 1:a7c5a3920ba8
- Parent:
- 0:0205108c2c99
30.10.2019 system now working
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
chanaka_madhusanka | 0:0205108c2c99 | 1 | #include "mbed.h" |
chanaka_madhusanka | 1:a7c5a3920ba8 | 2 | #include "SDFileSystem.h" |
chanaka_madhusanka | 1:a7c5a3920ba8 | 3 | #include "QEI.h" |
chanaka_madhusanka | 1:a7c5a3920ba8 | 4 | #include <math.h> |
chanaka_madhusanka | 1:a7c5a3920ba8 | 5 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 6 | SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board p5 - mosi, p6-miso, p7 - sck, p8 - cs |
chanaka_madhusanka | 1:a7c5a3920ba8 | 7 | Serial pc (USBTX,USBRX); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 8 | QEI wheel (p29, p30, NC, 2000, QEI::X4_ENCODING); //to get encoder position |
chanaka_madhusanka | 1:a7c5a3920ba8 | 9 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 10 | //int i= 2; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 11 | int value = 0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 12 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 13 | void PID(); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 14 | void motor(); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 15 | void DOB(double derivative_position); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 16 | void RTOB(double derivative_position); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 17 | void sendDAC(int value); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 18 | void file_writer(double input, double torque); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 19 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 20 | double dt=0.0002,t=0; //time |
chanaka_madhusanka | 1:a7c5a3920ba8 | 21 | double Kp=0,Ki=0,Kd=0; //pid |
chanaka_madhusanka | 1:a7c5a3920ba8 | 22 | double torque_rtob,rtob_filter_input, rtob_filter_output, rtob_filter_integral = 0, rtob_additional_torque; // |
chanaka_madhusanka | 1:a7c5a3920ba8 | 23 | double torque_dob, dob_filter_input, dob_filter_output, dob_filter_integral = 0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 24 | 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; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 25 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 26 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 27 | //DAC 712 |
chanaka_madhusanka | 1:a7c5a3920ba8 | 28 | DigitalOut dac_a0(p13); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 29 | DigitalOut dac_a1(p12); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 30 | DigitalOut dac_wr(p9); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 31 | DigitalOut dac_clr(p10); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 32 | DigitalOut dac_d15(p15); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 33 | DigitalOut dac_d14(p16); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 34 | DigitalOut dac_d13(p17); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 35 | DigitalOut dac_d12(p18); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 36 | DigitalOut dac_d11(p19); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 37 | DigitalOut dac_d10(p20); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 38 | DigitalOut dac_d9(p21); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 39 | DigitalOut dac_d8(p22); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 40 | DigitalOut dac_d7(p23); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 41 | DigitalOut dac_d6(p24); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 42 | DigitalOut dac_d5(p25); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 43 | DigitalOut dac_d4(p26); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 44 | DigitalOut dac_d3(p27); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 45 | DigitalOut dac_d2(p28); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 46 | DigitalOut dac_d1(p11); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 47 | DigitalOut dac_d0(p14); |
chanaka_madhusanka | 0:0205108c2c99 | 48 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 49 | int main(){ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 50 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 51 | float val = 32767/10; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 52 | int sendval; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 53 | double position; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 54 | double x; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 55 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 56 | //while(1){ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 57 | //reverse loop |
chanaka_madhusanka | 1:a7c5a3920ba8 | 58 | dac_a0 = 0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 59 | dac_a1 = 1; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 60 | dac_clr = 1; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 61 | 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 |
chanaka_madhusanka | 1:a7c5a3920ba8 | 62 | sendDAC(sendval); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 63 | dac_a0 = 1; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 64 | dac_a1 = 0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 65 | wait(0.1); //to change the back velocity |
chanaka_madhusanka | 1:a7c5a3920ba8 | 66 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 67 | dac_a0 = 0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 68 | dac_a1 = 1; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 69 | dac_clr = 1; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 70 | 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 |
chanaka_madhusanka | 1:a7c5a3920ba8 | 71 | sendDAC(sendval); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 72 | dac_a0 = 1; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 73 | dac_a1 = 0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 74 | wait(3); //to change the back velocity |
chanaka_madhusanka | 1:a7c5a3920ba8 | 75 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 76 | //forward loop |
chanaka_madhusanka | 1:a7c5a3920ba8 | 77 | dac_a0 = 0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 78 | dac_a1 = 1; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 79 | sendval = int(val * 0.06*(6)); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 80 | sendDAC(sendval); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 81 | dac_a0 = 1; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 82 | dac_a1 = 0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 83 | wait(4); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 84 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 85 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 86 | position = 0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 87 | x = -wheel.getPulses()/200.0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 88 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 89 | double torque; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 90 | torque = 25*0.6*0.4*(0.0284*(15)+0.1436)/1.2796; //maximum percentage is 40... range 0-40 |
chanaka_madhusanka | 1:a7c5a3920ba8 | 91 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 92 | while(-0.01>error_val | error_val>0.01) { //minimize the error value between reation torque and torque |
chanaka_madhusanka | 1:a7c5a3920ba8 | 93 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 94 | pc.printf("%lf\t %lf\t %lf\r\n",pid_val,torque_rtob,error_val); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 95 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 96 | position = (-wheel.getPulses()/200.0)-x; //////////////// |
chanaka_madhusanka | 1:a7c5a3920ba8 | 97 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 98 | double derivative_position = (position-pre_position)/dt; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 99 | pre_position = position; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 100 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 101 | error_val = torque - torque_rtob; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 102 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 103 | PID(); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 104 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 105 | //DOB(derivative_position); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 106 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 107 | RTOB(derivative_position); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 108 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 109 | motor_voltage = ((pid_val*J)/25)*(1.2796/(0.6*0.4)); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 110 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 111 | //motor_input = motor_voltage + ((torque_dob/25)*(1.2796/(0.6*0.4))); //sendVal=motor_input |
chanaka_madhusanka | 1:a7c5a3920ba8 | 112 | motor_input = motor_voltage; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 113 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 114 | dac_a0 = 0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 115 | dac_a1 = 1; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 116 | sendval = motor_input; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 117 | sendDAC(sendval); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 118 | dac_a0 = 1; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 119 | dac_a1 = 0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 120 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 121 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 122 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 123 | //fprintf (fp3, "%f %f %f\n",t,torque_rtob,load_torque); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 124 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 125 | t+=0.0002; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 126 | } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 127 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 128 | if((wheel.getPulses()/200.0)<6){ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 129 | //file_writer(position,torque_rtob); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 130 | } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 131 | pc.printf("Pulses is: %lf\t %lf\r\n",position,torque_rtob); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 132 | |
chanaka_madhusanka | 0:0205108c2c99 | 133 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 134 | //for(double i = 1 ; i<=20 ; i+=0.2){ //to increase torque |
chanaka_madhusanka | 1:a7c5a3920ba8 | 135 | // |
chanaka_madhusanka | 1:a7c5a3920ba8 | 136 | // double torque; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 137 | // |
chanaka_madhusanka | 1:a7c5a3920ba8 | 138 | // if(i<10.1){ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 139 | // |
chanaka_madhusanka | 1:a7c5a3920ba8 | 140 | // torque = 25*0.6*0.4*(0.0284*(6+i)+0.1436)/1.2796; //maximum percentage is 40... range 0-40 |
chanaka_madhusanka | 1:a7c5a3920ba8 | 141 | // |
chanaka_madhusanka | 1:a7c5a3920ba8 | 142 | // while(-0.1>error_val | error_val>0.1) { //minimize the error value between reation torque and torque |
chanaka_madhusanka | 1:a7c5a3920ba8 | 143 | // pc.printf("%lf\t %lf\t %lf\r\n",torque,torque_rtob,position); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 144 | // |
chanaka_madhusanka | 1:a7c5a3920ba8 | 145 | // position = (-wheel.getPulses()/200.0)-x; //////////////// |
chanaka_madhusanka | 1:a7c5a3920ba8 | 146 | // |
chanaka_madhusanka | 1:a7c5a3920ba8 | 147 | // double derivative_position = (position-pre_position)/dt; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 148 | // pre_position = position; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 149 | // |
chanaka_madhusanka | 1:a7c5a3920ba8 | 150 | // error_val = torque - torque_rtob; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 151 | // |
chanaka_madhusanka | 1:a7c5a3920ba8 | 152 | // PID(); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 153 | // |
chanaka_madhusanka | 1:a7c5a3920ba8 | 154 | // motor_voltage = ((pid_val*J)/25)*(1.2796/0.6*0.4); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 155 | // |
chanaka_madhusanka | 1:a7c5a3920ba8 | 156 | // motor_input = motor_voltage + ((torque_dob/25)*(1.2796/0.6*0.4)); //sendVal=motor_input |
chanaka_madhusanka | 1:a7c5a3920ba8 | 157 | // |
chanaka_madhusanka | 1:a7c5a3920ba8 | 158 | // dac_a0 = 0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 159 | // dac_a1 = 1; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 160 | // sendval = motor_input; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 161 | // sendDAC(sendval); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 162 | // dac_a0 = 1; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 163 | // dac_a1 = 0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 164 | // |
chanaka_madhusanka | 1:a7c5a3920ba8 | 165 | // DOB(derivative_position); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 166 | // RTOB(derivative_position); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 167 | // |
chanaka_madhusanka | 1:a7c5a3920ba8 | 168 | // //fprintf (fp3, "%f %f %f\n",t,torque_rtob,load_torque); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 169 | // |
chanaka_madhusanka | 1:a7c5a3920ba8 | 170 | // t+=0.0002; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 171 | // } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 172 | // |
chanaka_madhusanka | 1:a7c5a3920ba8 | 173 | // if((wheel.getPulses()/200.0)<6){ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 174 | // //file_writer(position,torque_rtob); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 175 | // } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 176 | // pc.printf("Pulses is: %lf\t %lf\r\n",position,torque_rtob); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 177 | // |
chanaka_madhusanka | 1:a7c5a3920ba8 | 178 | // if(i>9.9){ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 179 | // for(int j=1; j<500 ;j++){ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 180 | // pc.printf("Wait position : %lf\t %lf\r\n",-(wheel.getPulses()/200.0)-x,torque); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 181 | // } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 182 | // } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 183 | // }else if(i>=10.1){ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 184 | // |
chanaka_madhusanka | 1:a7c5a3920ba8 | 185 | // dac_a0 = 0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 186 | // dac_a1 = 1; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 187 | // sendval = int(val * 0.06*(6+20-i)); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 188 | // sendDAC(sendval); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 189 | // dac_a0 = 1; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 190 | // dac_a1 = 0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 191 | // torque = 25*0.6*0.4*(0.0284*(6+20-i)+0.1436)/1.2796 |
chanaka_madhusanka | 1:a7c5a3920ba8 | 192 | // position = (-wheel.getPulses()/200.0)-x; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 193 | // |
chanaka_madhusanka | 1:a7c5a3920ba8 | 194 | // if((wheel.getPulses()/200.0)<6){ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 195 | // //file_writer(position,torque); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 196 | // } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 197 | // pc.printf("Pulses is: %lf\t %lf\r\n",position,torque); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 198 | // } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 199 | // wait(0.05); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 200 | // } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 201 | //} |
chanaka_madhusanka | 0:0205108c2c99 | 202 | } |
chanaka_madhusanka | 0:0205108c2c99 | 203 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 204 | //set voltages to dac |
chanaka_madhusanka | 1:a7c5a3920ba8 | 205 | void sendDAC(int value){ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 206 | int remainder = 0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 207 | dac_clr = 0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 208 | wait_us(120); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 209 | dac_clr = 1; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 210 | //send output dac |
chanaka_madhusanka | 1:a7c5a3920ba8 | 211 | dac_wr = 1; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 212 | if(value >= 0){ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 213 | if(value >= 32767){ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 214 | 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; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 215 | } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 216 | else { |
chanaka_madhusanka | 1:a7c5a3920ba8 | 217 | dac_d15 = 0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 218 | dac_d14 = value / 16384 ; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 219 | remainder = value - dac_d14.read() * 16384; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 220 | dac_d13 = remainder / 8192; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 221 | remainder = remainder - dac_d13.read() * 8192; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 222 | dac_d12 = remainder / 4096; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 223 | remainder = remainder - dac_d12.read() * 4096; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 224 | dac_d11 = remainder / 2048; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 225 | remainder = remainder - dac_d11.read() * 2048; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 226 | dac_d10 = remainder / 1024; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 227 | remainder = remainder - dac_d10.read() * 1024; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 228 | dac_d9 = remainder / 512; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 229 | remainder = remainder - dac_d9.read() * 512; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 230 | dac_d8 = remainder / 256; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 231 | remainder = remainder - dac_d8.read() * 256; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 232 | dac_d7 = remainder / 128; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 233 | remainder = remainder - dac_d7.read() * 128; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 234 | dac_d6 = remainder / 64; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 235 | remainder = remainder - dac_d6.read() * 64; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 236 | dac_d5 = remainder / 32; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 237 | remainder = remainder - dac_d5.read() * 32; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 238 | dac_d4 = remainder / 16; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 239 | remainder = remainder - dac_d4.read() * 16; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 240 | dac_d3 = remainder / 8; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 241 | remainder = remainder - dac_d3.read() * 8; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 242 | dac_d2 = remainder / 4; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 243 | remainder = remainder - dac_d2.read() * 4; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 244 | dac_d1 = remainder / 2; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 245 | remainder = remainder - dac_d1.read() * 2; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 246 | dac_d0 = remainder ; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 247 | } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 248 | } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 249 | if (value < 0){ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 250 | if(value <= -32768){ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 251 | 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; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 252 | } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 253 | else{ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 254 | dac_d15 = 1; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 255 | dac_d14 = ( 32768 + value) / 16384 ; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 256 | remainder = ( 32768 + value) - dac_d14.read() * 16384; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 257 | dac_d13 = remainder / 8192; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 258 | remainder = remainder - dac_d13.read() * 8192; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 259 | dac_d12 = remainder / 4096; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 260 | remainder = remainder - dac_d12.read() * 4096; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 261 | dac_d11 = remainder / 2048; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 262 | remainder = remainder - dac_d11.read() * 2048; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 263 | dac_d10 = remainder / 1024; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 264 | remainder = remainder - dac_d10.read() * 1024; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 265 | dac_d9 = remainder / 512; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 266 | remainder = remainder - dac_d9.read() * 512; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 267 | dac_d8 = remainder / 256; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 268 | remainder = remainder - dac_d8.read() * 256; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 269 | dac_d7 = remainder / 128; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 270 | remainder = remainder - dac_d7.read() * 128; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 271 | dac_d6 = remainder / 64; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 272 | remainder = remainder - dac_d6.read() * 64; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 273 | dac_d5 = remainder / 32; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 274 | remainder = remainder - dac_d5.read() * 32; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 275 | dac_d4 = remainder / 16; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 276 | remainder = remainder - dac_d4.read() * 16; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 277 | dac_d3 = remainder / 8; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 278 | remainder = remainder - dac_d3.read() * 8; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 279 | dac_d2 = remainder / 4; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 280 | remainder = remainder - dac_d2.read() * 4; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 281 | dac_d1 = remainder / 2 ; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 282 | remainder = remainder - dac_d1.read() * 2; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 283 | dac_d0 = remainder ; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 284 | } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 285 | } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 286 | dac_wr = 0; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 287 | } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 288 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 289 | void file_writer(double position,double torque){ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 290 | mkdir("/sd/mydir", 0777); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 291 | FILE *fp = fopen("/sd/mydir/sdtesthys.txt", "a"); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 292 | if(fp == NULL){ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 293 | error("Could not open file for write\n"); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 294 | } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 295 | fprintf(fp,"%lf\t %lf\t \r\n",position,torque); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 296 | fclose(fp); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 297 | } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 298 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 299 | //PID controller |
chanaka_madhusanka | 1:a7c5a3920ba8 | 300 | void PID(){ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 301 | integral1 = integral1 + error_val*dt; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 302 | derivative_val = (error_val - pre_val)/dt; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 303 | pre_val = error_val; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 304 | pid_val = (0*error_val)+(0*integral1)+(0*derivative_val); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 305 | } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 306 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 307 | //disturbance observer |
chanaka_madhusanka | 1:a7c5a3920ba8 | 308 | void DOB(double derivative_position){ //disturbance observer |
chanaka_madhusanka | 1:a7c5a3920ba8 | 309 | 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 |
chanaka_madhusanka | 1:a7c5a3920ba8 | 310 | dob_filter_integral = dob_filter_integral + ((dob_filter_input - dob_filter_output) * dt); //filter |
chanaka_madhusanka | 1:a7c5a3920ba8 | 311 | dob_filter_output = g_dis * dob_filter_integral; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 312 | torque_dob = dob_filter_output - (Jn * g_dis); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 313 | } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 314 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 315 | //reaction torque observer |
chanaka_madhusanka | 1:a7c5a3920ba8 | 316 | void RTOB(double derivative_position){ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 317 | rtob_additional_torque = 1.10745; //ask sir |
chanaka_madhusanka | 1:a7c5a3920ba8 | 318 | rtob_filter_input = (derivative_position * g_dis * Jn) + (motor_input * Kt2 * (0.6*0.4/1.2796)) - rtob_additional_torque; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 319 | rtob_filter_integral = rtob_filter_integral + ((rtob_filter_input - rtob_filter_output) * dt); //fil |
chanaka_madhusanka | 1:a7c5a3920ba8 | 320 | rtob_filter_output = g_dis * rtob_filter_integral; |
chanaka_madhusanka | 1:a7c5a3920ba8 | 321 | torque_rtob = rtob_filter_output - (derivative_position * g_dis * Jn); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 322 | } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 323 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 324 | //Threading code |
chanaka_madhusanka | 1:a7c5a3920ba8 | 325 | void thread_2(void const *argument){ |
chanaka_madhusanka | 0:0205108c2c99 | 326 | while(1){ |
chanaka_madhusanka | 1:a7c5a3920ba8 | 327 | //pc.printf("%i\t %f %f\t \r\n)",count,dx_res,I_msrd); |
chanaka_madhusanka | 1:a7c5a3920ba8 | 328 | //file_writer(); |
chanaka_madhusanka | 0:0205108c2c99 | 329 | } |
chanaka_madhusanka | 0:0205108c2c99 | 330 | } |
chanaka_madhusanka | 1:a7c5a3920ba8 | 331 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 332 | |
chanaka_madhusanka | 1:a7c5a3920ba8 | 333 |