File content as of revision 1:32030d9ba873:

/*INCLUDE*/
#include "SDFileSystem.h"
#include "mbed.h"
#include "rotary_inc.hpp"
#include "matrix.h"
#include "BNO055.h"
#include "NK_PID.hpp"
#include <cmath>
#include <vector>
#include "scrp_slave.hpp"

ScrpSlave sendpwm(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800);

struct pos{
    double x;
    double y;
};
/*FUNCTIONS*/
void route_read();
void odmetry();
void move(double vx,double vy,double vw);
void getpoint(pos *p,int n);
void getvelocity(double *v,int n);

/*DECLARATION*/
SDFileSystem sd(PB_15, PB_14, PB_13, PB_2,"sd");

RotaryInc cenc0(PC_10,PC_11,2*M_PI,256,4);
RotaryInc cenc1(PC_4,PA_13,2*M_PI,256,4);
RotaryInc cenc2(PA_14,PA_15,2*M_PI,256,4);
RotaryInc cenc3(PC_2,PC_3,2*M_PI,256,4);

RotaryInc senc0(PA_6,PA_7,2*M_PI,256,4);
RotaryInc senc1(PA_8,PA_9,2*M_PI,256,4);
RotaryInc senc2(PC_0,PC_1,2*M_PI,256,4);
RotaryInc senc3(PA_12,PC_5,2*M_PI,256,4);

std::vector<double> t_info,x_info,y_info,v_info,theta_info;

/*
01
32
*/

Nk_pid mot0(0.0067,0.001,0,0.01);
Nk_pid mot1(0.0067,0.001,0,0.01);
Nk_pid mot2(0.0067,0.001,0,0.01);
Nk_pid mot3(0.0067,0.001,0,0.01);

BNO055 bno(PB_3, PB_10);

Timer LOOPdt;
unsigned long long int t;

pos a,b,c;
double v = 0;
double cwv[4];
double swv[4];
double theta = 0;
double r = 50.6;
double R = 2.0*r;
double cR = 322.5;
int n = 0;

PwmOut asd(PC_6);

int main(){
asd = 1;
/*SETUP*/
LOOPdt.start();
/*
*/
mot0.SetLimit(0.5,0.00,-0.5,-0.00);
mot1.SetLimit(0.5,0.00,-0.5,-0.00);
mot2.SetLimit(0.5,0.00,-0.5,-0.00);
mot3.SetLimit(0.5,0.00,-0.5,-0.00);
/*0.07*/
    
//route_read();
odmetry();
/*LOOP*/
wait(3);
while(true){
    
    odmetry();
    /*
    if(sqrt( (a.x-c.x)*(a.x-c.x)+(a.y-c.y)*(a.y-c.y) ) < 30){
        getpoint(&a,n);
        getvelocity(&v,n);
        if(n<x_info.size()) n++;
        printf("pass\n");
    }
    double theta = atan2(a.y-c.y,a.x-c.x);
    move(v*cos(theta),v*sin(theta),0);*/
    //move(0.0,2000.0,0.0);
    //motor0.pwmout(0.2);
    //printf("%lf\n",cwv[0]*r);
    
    sendpwm.send1(255, 1 , -0.3 * 100);
    while(LOOPdt.read_us()-t <= 10*1000/100*50);
    sendpwm.send1(255, 2 , 0.3 * 100);/*
    while(LOOPdt.read_us()-t <= 10*1000/100*70);
    sendpwm.send1(255, 3 , 0.3 * 100);
    while(LOOPdt.read_us()-t <= 10*1000/100*90);
    sendpwm.send1(255, 4 , 0.3 * 100);*/
    while(LOOPdt.read_us()-t <= 10*1000);
    t = LOOPdt.read_us();
}

}

void route_read() {
    static double x, y, v, theta;
    wait(1);
    pc.printf("Hello World!\n");
    wait(0.1);
    FILE *fp;
    do {
        fp = fopen("/sd/route.txt", "r");
        wait(0.2);
    } while(fp==NULL);
    pc.printf("Good!\n");
    while(fscanf(fp,"%lf %lf %lf %lf",&x, &y, &v, &theta) != EOF) {
        pc.printf("%lf %lf %lf %lf\n",x, y, v, theta);
        x_info.push_back(x);
        y_info.push_back(y);
        v_info.push_back(v);
        theta_info.push_back(theta);
    }
    pc.printf("checking...\n");
    for(int k=0;k<x_info.size();k++){
        pc.printf("%lf %lf %lf %lf\n",x_info[k],y_info[k],v_info[k],theta_info[k]);
    }
    fclose(fp);
}

void odmetry(){
    
    static Matrix<double> Vr(3, 1);
    static Matrix<double> J(3, 4);
    static Matrix<double> W(4, 1);
    static double gyro = 0.0;
    static double x = 0.0;
    static double y = 0.0;
    static double vx = 0.0;
    static double vy = 0.0;
    static double sold0 = 0.0, sold1 = 0.0, sold2 = 0.0, sold3 = 0.0;
    static double cold0 = 0.0, cold1 = 0.0, cold2 = 0.0, cold3 = 0.0;
    double cor0 = 0.0, cor1 = 0.0, cor2 = 0.0, cor3 = 0.0;
    double side0 = 0.0, side1 = 0.0, side2 = 0.0, side3 = 0.0;
    static bool setup = false;
    if(!setup){
        //bno.setmode(OPERATION_MODE_IMUPLUS);
        bno.reset();
        bno.setmode(OPERATION_MODE_IMUPLUS);
        bno.get_angles();
        /*while(fgyro - (bno.euler.yaw * (M_PI / 180.0)) != 0.0){
            fgyro = bno.euler.yaw * (M_PI / 180.0);
            bno.get_angles();
        }*/
        setup = true;
    }
    
    cor0 = M_PI*cenc0.get()/1024.0;
    cor1 = M_PI*cenc1.get()/1024.0;
    cor2 = M_PI*cenc2.get()/1024.0;
    cor3 = M_PI*cenc3.get()/1024.0;
    cwv[0] = (cor0-cold0)/0.01;
    cwv[1] = (cor1-cold1)/0.01;
    cwv[2] = (cor2-cold2)/0.01;
    cwv[3] = (cor3-cold3)/0.01;
    side0 = M_PI*senc0.get()/1024.0;
    side1 = M_PI*senc1.get()/1024.0;
    side2 = M_PI*senc2.get()/1024.0;
    side3 = M_PI*senc3.get()/1024.0;
    swv[0] = (side0-sold0)/0.01;
    swv[1] = (side1-sold1)/0.01;
    swv[2] = (side2-sold2)/0.01;
    swv[3] = (side3-sold3)/0.01;
    
    cold0 = cor0;
    cold1 = cor1;
    cold2 = cor2;
    cold3 = cor3;
    
    sold0 = side0;
    sold1 = side1;
    sold2 = side2;
    sold3 = side3;
    
    W.set(  4,
            cwv[0],
            cwv[1],
            cwv[2],
            cwv[3]
    );
    /*
    W.set(  4,
            swv[0],
            swv[1],
            swv[2],
            swv[3]
    );*/
    
    //W.show();
    bno.get_angles();
    gyro = bno.euler.yaw * (M_PI / 180.0);
    theta = (2*M_PI-gyro);
    /*
    J.set(  12,
            -sin(-theta + M_PI/4.0), -sin(-theta + M_PI/2.0 + M_PI/4.0), -sin(-theta + M_PI + M_PI/4.0), -sin(-theta + 3.0*M_PI/2.0 + M_PI/4.0),
             -cos(-theta + M_PI/4.0),  -cos(-theta + M_PI/2.0 + M_PI/4.0),  -cos(-theta + M_PI + M_PI/4.0),  -cos(-theta + 3.0*M_PI/2.0 + M_PI/4.0),
            1.0/(2.0*R),            1.0/(2.0*R),        1.0/(2.0*R),                1.0/(2.0*R)
    );
    */
    
    J.set(  12,
            -sin(-theta - 2*M_PI/4), -sin(-theta + M_PI/2.0 - 2*M_PI/4), -sin(-theta + M_PI - 2*M_PI/4), -sin(-theta + 3.0*M_PI/2.0 - 2*M_PI/4),
             -cos(-theta - 2*M_PI/4),  -cos(-theta + M_PI/2.0 - 2*M_PI/4),  -cos(-theta + M_PI - 2*M_PI/4),  -cos(-theta + 3.0*M_PI/2.0 - 2*M_PI/4),
            1.0/(2.0*R),            1.0/(2.0*R),        1.0/(2.0*R),                1.0/(2.0*R)
    );
    //J.show();

    Vr = J*W*(r/2);
    //Vr.show();
    
    x += ((vx + Vr[0][0]) * 0.01) / 2.0;
    y += ((vy + Vr[1][0]) * 0.01) / 2.0;
    vx = Vr[0][0];
    vy = Vr[1][0];
    c.x = x;
    c.y = y;
    pc.printf("x : %lf y : %lf  vr : %lf theta : %lf\r\n", x, y, Vr[2][0], theta);
    //pc.printf("vx : %lf vy : %lf\r\n",Vr[0][0], Vr[1][0]);
        
}

void getpoint(pos *p,int n){
    p->x = x_info[n];
    p->y = y_info[n];
}

void getvelocity(double *v,int n){
    *v = v_info[n];
}

void move(double vx,double vy,double vw){
    
    static double k = sqrt(2.0)/2.0;;
    static double target[4];
    
    target[0]= vx*k*(sin(theta)-cos(theta)) - vy*k*(sin(theta)+cos(theta)) + cR*vw;
    target[1]= -vx*k*(sin(theta)+cos(theta)) - vy*k*(sin(theta)-cos(theta)) + cR*vw;
    target[2]= -vx*k*(sin(theta)-cos(theta)) + vy*k*(sin(theta)+cos(theta)) + cR*vw;
    target[3]= vx*k*(sin(theta)+cos(theta)) + vy*k*(sin(theta)-cos(theta)) + cR*vw;
    
    /*
      target[1]=(k*vy-k*vx)+cR*vw;
      target[0]=(k*vx+k*vy)*-1;
      target[3]=(k*vx-k*vy)+cR*vw;
      target[2]=(k*vx+k*vy)+cR*vw;*/
    mot0.SetParam(cwv[0]*r/100.0,target[0]/100.0);
    mot1.SetParam(cwv[1]*r/100.0,target[1]/100.0);
    mot2.SetParam(cwv[2]*r/100.0,target[2]/100.0);
    mot3.SetParam(cwv[3]*r/100.0,target[3]/100.0);
    
    //printf("%lf\n",mot0.GetGain(1.0));
    //motor0.pwmout(mot0.GetGain(1.0)
    sendpwm.send1(255, 1 ,(mot0.GetGain(1.0) * 100));
    while(LOOPdt.read_us()-t <= 10*1000/100*40);
    sendpwm.send1(255, 2 ,(mot1.GetGain(1.0) * 100));
    while(LOOPdt.read_us()-t <= 10*1000/100*70);
    sendpwm.send1(255, 3 ,(mot2.GetGain(1.0) * 100));
    while(LOOPdt.read_us()-t <= 10*1000/100*90);
    sendpwm.send1(255, 4 ,(mot3.GetGain(1.0) * 100));
    //printf("%lf,%lf\n",cwv[0]*r,target[0]);
}