File content as of revision 12:ed227eb82214:

#define USESCRP
#include "core.hpp"
#include "SDFileSystem.h"
#include "BNO055.h"
#include <cmath>
#include <vector>
#include <ros.h>
#include <std_msgs/Int16.h>

#include "mbed.h"
#define POINTS 220
#define ISLAND 190
#define PARK 70
const double X0 = 442.5;
const double Y0 = 628.5;


/*--------------FUNCTION--------------*/
bool init();
void route_read();
void spinonce();
void getPoint(Position *p,int n);
void getVelocity(double *v,int n);
bool incircle(Position pos,Position target,double error);
void gyroA(double theta);
void gyroB(double theta);
bool checkMOT12(int rx_data, int &tx_data);
bool checkMOT34(int rx_data, int &tx_data);
void getPixel(const std_msgs::Int16& pix);


/*--------------VARIABLE--------------*/

std::vector<double> t_info,x_info,y_info,v_info,theta_info;
Position pos,vel,NextP,subP;
int n=1;
int cnt = 0;
double v=0.0;
double theta = 0.0;
double gyro = 0.0;
int pixel = 0.0;
bool gyro_1 = false,gyro_2 = false;
long long g_pulse;
bool MOT12 = false,MOT34 = false;
bool ros_ = false;
BNO055 bno(PB_3, PB_10);

ros::NodeHandle nh;
Ticker rosspin;

SDFileSystem sd(PB_15, PB_14, PB_13, PB_1,"sd");// mosi, miso, sclk, cs
//ScrpSlave scrp(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800);
ScrpSlave scrp(PC_12,PD_2 ,PH_1 ,0x0807f800);
Robot AKASHIKOSEN(50.8,25.4,322.5,259.75);
Core RBT(&AKASHIKOSEN,&scrp,OMNI4,0.02);

ros::Subscriber<std_msgs::Int16> sub("unti", &getPixel);

int x=1;

int main(){

    DigitalOut hoge(PB_2);
    hoge = x;  
      
    PwmOut servo(PA_0);
    PwmOut ok(PC_6);
    DigitalIn Lim_R(PB_0,PullDown);
    DigitalIn Lim_L(PA_4,PullDown);

/*--------------SETUP--------------*/
    servo.period_ms(2048);
    servo.pulsewidth_us(1500);
    
    nh.getHardware()->setBaud(115200);
    nh.initNode();
    nh.subscribe(sub);
    rosspin.attach_us(&spinonce,1000);
        
    scrp.addCMD(7,checkMOT12);
    scrp.addCMD(8,checkMOT34);
    AKASHIKOSEN.setCWID(1,2,3,4);
    AKASHIKOSEN.setSWID(5,6,7,8);
        
    RBT.addENC(PC_4,PA_13,512,4,1);
    RBT.addENC(PA_14,PA_15,512,4,2);
    RBT.addENC(PC_2,PC_3,512,4,3);
    RBT.addENC(PC_10,PC_11,512,4,4);
    RBT.addENC(PA_7,PA_6,512,4,5);
    RBT.addENC(PA_9,PA_8,512,4,6);
    RBT.addENC(PC_1,PC_0,512,4,7);
    RBT.addENC(PC_5,PA_12,512,4,8);
    
    RBT.addPID(0.0048,0.001,0.00008,1)->setLimit(0.5,-0.5);
    RBT.addPID(0.0048,0.001,0.00008,2)->setLimit(0.5,-0.5);
    RBT.addPID(0.0048,0.001,0.00008,3)->setLimit(0.5,-0.5);
    RBT.addPID(0.0048,0.001,0.00008,4)->setLimit(0.5,-0.5);
    RBT.addPID(0.8,0.00,0.0,5)->setLimit(2.0*M_PI/4.0,-2.0*M_PI/4.0);
    RBT.addPID(0.007,0.00,0.0,0)->setLimit(2.0*M_PI/16.0,-2.0*M_PI/16.0);
    //RBT.addPID(0.5,0.001,0.2,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0);
    /*
    RBT.addPID(0.0048,0.00,0.0000,1)->setLimit(0.5,-0.5);
    RBT.addPID(0.0048,0.00,0.0000,2)->setLimit(0.5,-0.5);
    RBT.addPID(0.0048,0.00,0.00,3)->setLimit(0.5,-0.5);
    RBT.addPID(0.0048,0.00,0.00,4)->setLimit(0.5,-0.5);
    RBT.addPID(0.5,0.001,0.2,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0);
    */
    MOT12 = false,MOT34 = false;
    
    while(init());
    printf("SCRP_CHECKING MOT12:%d,MOT34:%d\n",MOT12,MOT34);
    
    route_read();
    
    wait(1);
    ok = 1.0;
    
    bool setup = false;
    bool once = false;
    bool xy_stop = false;
    bool rad_stop = false;
    if(!setup){
        bno.reset();
        bno.setmode(OPERATION_MODE_IMUPLUS);
        setup = true;
    }
        
    RBT.START();
    RBT.setPosition(0.0,0.0,0.0);
    /*--------------LOOP--------------*/
    while(Lim_R == 1 and Lim_L == 1){};
    while(true){
        hoge = x;
        pos = RBT.getStatus(true);
        bno.get_angles();
        gyro = 2.0*M_PI - bno.euler.yaw * (M_PI / 180.0);
        gyroA(gyro);
        gyroB(gyro);
        
        theta = 2.0*M_PI*(int)(g_pulse/4.0);
        if(g_pulse > 0.0) theta += gyro;
        else if(g_pulse < 0.0) theta += gyro-2.0*M_PI;
        else if(g_pulse == 0 and gyro < M_PI/2.0) theta += gyro;
        else if(g_pulse == 0 and gyro > 3.0*M_PI/2.0) theta += gyro-2.0*M_PI;
        RBT.setPosition(pos.x,pos.y,theta);
        if(!once){
            RBT.setPosition(X0,Y0,0.0);
            once = true;
        }
        
        //RBT.setVelocity(0.0,0.0,0.0);
        if(n<POINTS and 
            ( (incircle(pos,NextP,50.0) and n < POINTS  and n != ISLAND and n != PARK) or 
                ( incircle(pos,NextP,10.0) and n == PARK-1 ) or
                ( sqrt( (NextP.x-pos.x)*(NextP.x-pos.x)+(NextP.y-pos.y)*(NextP.y-pos.y) ) > sqrt( (subP.x-pos.x)*(subP.x-pos.x)+(subP.y-pos.y)*(subP.y-pos.y) ) and n < POINTS and n != ISLAND and n != PARK)
            ) 
        )
        {
            n++;
            getPoint(&NextP,n);
            if(n < 10) NextP.theta = 0.0;
            getVelocity(&v,n-1);
            if(n < POINTS)getPoint(&subP,n+1);
            if(v > 0.0) v *= 1.0;
            if(n < PARK) v*= 1.9;
            if(n < 120 and n > PARK) v*= 2.0;
            if(160 > n and n >= 120) v*= 1.7;
            if(180 > n and n >= 160) v*= 2.5;
            if(n >= 180) v*= 3.0;
            //if(n > 700) v = 300;
            //v = 20.0;
            if(n == POINTS) v = 100.0;
            printf("%d:pass\n",n);
        }
        
        double etheta = atan2(NextP.y-pos.y,NextP.x-pos.x);
        RBT.PIDs[5]->Update(pos.theta,NextP.theta,0.02,false);
        vel.theta = RBT.PIDs[5]->getmv();
        
        xy_stop = false;
        rad_stop = false;

        RBT.PIDs[0]->Update(pixel,0.1,0.02,false);
        if(n < ISLAND or n == POINTS) servo.pulsewidth_us(1500);
        if(n == ISLAND and incircle(pos,NextP,30.0) and abs(pixel) > 40){
            xy_stop = true;
            vel.theta = RBT.PIDs[0]->getmv();
            cnt = 0;
        }
        else if(n == ISLAND and incircle(pos,NextP,30.0)){
            cnt++;
            xy_stop = true;
            rad_stop = true;
            if((double)cnt > 2.0/0.02) {v=300.0;servo.pulsewidth_us(1170);n++;}
            }
        if(n > ISLAND and n <= ISLAND + 5) {
            cnt++;
            if((double)cnt < 4.0/0.02) {v=300.0;servo.pulsewidth_us(1170);xy_stop = true;rad_stop = true;}
        }
        
        if(n == POINTS and incircle(pos,NextP,10.0)) xy_stop = true;
        if(n == POINTS and fabs(NextP.theta - pos.theta) < 0.1) rad_stop = true;;
        
        vel.x = v*cos(etheta);
        vel.y = v*sin(etheta);
        
        if(n == PARK and Lim_R == 1 and Lim_L == 1) {vel.x = 100.0; vel.y = 0.0; ok = 1;cnt = 0;}
        else if(n == PARK) {xy_stop = true;rad_stop = true;ok = 0;cnt++;if((double)cnt > 12.0/0.02) {v=300.0;cnt = 0;n++;}}
        
        if(xy_stop) {vel.x = 0.0;vel.y = 0.0;}
        if(rad_stop) {vel.theta = 0.0;}
        
        RBT.sendVelocity(vel.x,vel.y,vel.theta);
        //printf("L:%d R:%d\n",Lim_L,Lim_R);
        printf("x:%lf,y:%lf,theta:%lf,n:%d\n",pos.x,pos.y,pos.theta,n);
        nh.spinOnce();
        RBT.LOOP();
    }
}

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

void getPoint(Position *p,int n){
    p->x = x_info[n];
    p->y = y_info[n];
    p->theta = theta_info[n];
}

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

bool incircle(Position pos,Position target,double error){
    return (sqrt( (target.x-pos.x)*(target.x-pos.x)+(target.y-pos.y)*(target.y-pos.y) ) < error);
}
void gyroA(double theta){
    static bool prev = false;
    if(0 <= theta and theta < M_PI) gyro_1 = false;
    else gyro_1 = true;
    if(gyro_1 and !prev){
        if(gyro_2) g_pulse++;
        else if(g_pulse != 0) g_pulse--;
    }
    else if(!gyro_1 and prev){
        if(gyro_2) g_pulse--;
        else if(g_pulse != 0)g_pulse++;
    }
    prev = gyro_1;
}
void gyroB(double theta){
    static bool prev = false;
    if((3.0*M_PI/2 <= theta and theta <= 2*M_PI) or (theta < M_PI/2.0)) gyro_2 = false;
    else gyro_2 = true;
    if(gyro_2 and !prev){
        if(gyro_1) g_pulse--;
        else g_pulse++;
    }
    else if(!gyro_2 and prev){
        if(gyro_1) g_pulse++;
        else g_pulse--;
    }
    prev = gyro_2;
}

bool checkMOT12(int rx_data, int &tx_data){
    MOT12 = true;
    return true;
}
bool checkMOT34(int rx_data, int &tx_data){
    MOT34 = true;
    return true;
}

bool init(){
    wait(0.25);
    scrp.send1(255,5,1);
    wait(0.25);
    scrp.send1(255,6,1);
    printf("aaaa");
    return (!MOT12 or !MOT34);
}
void spinonce()
{
    nh.spinOnce();
}

void getPixel(const std_msgs::Int16& pix){
    pixel = pix.data;
    ros_ = true;
    if (pixel > 0){
        x = 0;    
    } else {
        x = 1;
    }
}