File content as of revision 1:a7c5a3920ba8:

#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); 

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);
                
 
        //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); 
//        }        
    //}
}

//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){
        //pc.printf("%i\t %f %f\t \r\n)",count,dx_res,I_msrd);
        //file_writer();
    }
}