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

--- /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

--- 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
+
+
+