Chanaka Madhusanka / Mbed 2 deprecated G19_Final_Code_For_Testing_notworking

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?

UserRevisionLine numberNew 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