Diff: main.cpp

Revision:

0:ec993927eafa

Child:

2:e026bb894dcd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jan 11 08:01:57 2019 +0000
@@ -0,0 +1,454 @@
+#include "mbed.h"
+#include "S11059.h"
+#include "PCA9547.h"
+#include "Motor.h"
+#include "Motor_con.h"
+#include "QEI.h"
+#include "PID.h"
+
+#define Limit_angularvelocity 1.0
+#define Kp 0.36
+#define Ki 0.156
+#define Kd 0.039
+#define RADIUS 0.0635 //radius
+#define GREEN1 12
+#define GREEN 4 //Line detection green
+#define DEVICE_ADDRESS 0x2A << 1  //I2C Address
+#define CONTROL_REG 0x00 //control register
+#define TIMING_REG_H 0x01 //high byte of timing register
+#define TIMING_REG_L 0x02 //low byte of timing register
+
+DigitalOut Val(LED1);
+DigitalOut LED(LED2);
+DigitalOut INa(p6);
+DigitalOut INb(p5);
+DigitalOut PSB(p20);
+
+Timer *timer = new Timer();
+
+Serial pc(USBTX,USBRX);
+PCA9547 mux(p28,p27,0xE0);
+I2C i2c(p28,p27);
+
+Motor up_motor(p26,p19);
+Motor down_motor(p25,p10);
+Motor right_motor(p24,p9);
+Motor left_motor(p23,p8);
+Motor_Con motor_con(RADIUS);
+Motor ang(p22,p7);
+
+QEI qei_u(p13,p12,NC,48*71,timer,QEI::X2_ENCODING);
+QEI qei_d(p15,p14,NC,48*71,timer,QEI::X2_ENCODING);
+QEI qei_r(p17,p16,NC,48*71,timer,QEI::X2_ENCODING);
+QEI qei_l(p11,p18,NC,48*71,timer,QEI::X2_ENCODING);
+
+PID pid_u;
+PID pid_d;
+PID pid_r;
+PID pid_l;
+
+char cmd[3] = {CONTROL_REG,TIMING_REG_H,TIMING_REG_L};
+char ord[2] = {};
+char data[8] = {};
+char p[3] = {};
+int stage=0,s=1;
+int addra = 0x02;
+int addrb = 0x04;
+int a=0,b=0,c=0;
+int Linecount_str=0;
+int Situation=0;
+int now=0;
+int cal_g[6]={0,0,0,0,0,0};
+uint16_t red[6] = {0,0,0,0,0,0};
+uint16_t green[6] = {0,0,0,0,0,0};
+uint16_t prev_g[6] = {0,0,0,0,0,0};
+uint16_t pprev_g[6] = {0,0,0,0,0,0};
+uint16_t blue[6] = {0,0,0,0,0,0};
+uint16_t IR = 0;
+uint8_t address = DEVICE_ADDRESS;
+double dis_avr=0;
+double data_set[4]={0,0,0,0};
+double data_spd[4]={0,0,0,0};
+double RPS[4]={0,0,0,0};
+double control[4]={0,0,0,0};
+double x=0,y=0;
+double angle1=qei_u.getSumangle();
+double angle2=qei_d.getSumangle();
+double angle3=qei_r.getSumangle();
+double angle4=qei_l.getSumangle();
+
+void init();
+void run();
+void motorstop();
+void Arduino1();
+void Arduino2(int d);
+void Linetrace(); //Linetrace
+void Cal_X(); //calculate moving distance of X axis
+void Cal_Y(); //calculate moving distance of Y axis
+void getRGB(); //Acquire measurement result of color sensor
+void Adjustment();
+void Injection(int i);
+void jud();
+
+Ticker inter; //interval timer
+
+void Sensor(void){
+    int i;
+    for(i=0;i<6;i++){
+        pprev_g[i]=prev_g[i];
+        prev_g[i]=green[i];
+    }
+    getRGB();
+    for(i=0;i<6;i++){
+        cal_g[i]=green[i]-pprev_g[i];
+    }
+        
+    /*if(Situation==0 && GREEN1<cal_g[1]){
+        Situation=1;
+    }//ue ga yonndara zyotai1 ni suru*/
+    
+    if(Situation == 0 && GREEN<cal_g[2]){//right up
+        Situation = 1;
+    }
+    if(Situation == 0 && GREEN<cal_g[3]){//left up
+        Situation = 2;
+    }
+    if(Situation == 1 && GREEN<cal_g[3]){
+        Situation = 3;
+    }
+    if(Situation == 2 && GREEN<cal_g[2]){
+        Situation = 3;
+    }
+  
+    /*if(Situation==1 && GREEN1<cal_g[1]){
+        Situation=0;
+        Linecount_str++;
+    }*/
+}
+
+int main(){
+    int i=0,j;
+    i2c.frequency(400000);
+    //jud();
+    PSB=0;
+    
+    /*for(j=0;j<50;j++){
+        //pc.printf("wait...");
+    }*/
+    
+    inter.attach(&Sensor, 0.3); //interval timer interrupt 200ms
+    qei_u.qei_reset();
+    qei_d.qei_reset();
+    qei_r.qei_reset();
+    qei_l.qei_reset();
+    pid_u.setParameter_pid(Kp,Ki,Kd);
+    pid_d.setParameter_pid(Kp,Ki,Kd);
+    pid_r.setParameter_pid(Kp,Ki,Kd);
+    pid_l.setParameter_pid(Kp,Ki,Kd);
+    
+    
+    while(1){
+        Arduino1();
+        for(i=1;i<6;i++){
+            //pc.printf(", %3d, %3d, %3d\r\n",red[i],green[i],blue[i]);
+            pc.printf(", %3d, %3d, %3d\r\n",p[0],p[1],p[2]);
+        }
+        pc.printf("\n");
+        motor_con.setspeed(0,-0.5,0);
+        motor_con.getspeed(data_set);
+        init();
+        while(Linecount_str==0){
+        //while(1){
+            Cal_Y();
+            run();
+        }//sen4hon yomumade ueidou
+        while(p[stage]<8){
+            Linetrace();
+            Cal_X();
+        }
+        motor_con.stop();
+        motorstop();
+        //wait(10);
+        PSB=1;
+        Injection(i);
+        
+        motor_con.setspeed(0.5*s,0,0);
+        motor_con.getspeed(data_set);
+        init();
+        while(x>10){
+            Cal_X();
+            run();
+        }//hidariidou
+
+        motor_con.setspeed(0,0.5,0);
+        motor_con.getspeed(data_set);
+        init();
+        while(y>10){
+            Cal_Y();
+            run();
+        }//sitaidou
+        
+        Linecount_str=0;
+        i++;
+        motor_con.stop();
+        motorstop();
+        //wait(10);
+    }
+}
+
+void init(){
+    int i;
+    up_motor.speed(data_set[0]);
+    down_motor.speed(data_set[1]);
+    right_motor.speed(data_set[2]);
+    left_motor.speed(data_set[3]);
+    for(i=0;i<4;i++){
+        data_spd[i]=data_set[i];
+    }
+}
+
+void run(){
+    int i;
+    RPS[0]=qei_u.getRPS();
+    RPS[1]=qei_d.getRPS();
+    RPS[2]=qei_r.getRPS();
+    RPS[3]=qei_l.getRPS();
+    control[0]=pid_u.control(data_set[0],RPS[0]/Limit_angularvelocity,timer);
+    control[1]=pid_d.control(data_set[1],RPS[1]/Limit_angularvelocity,timer);
+    control[2]=pid_r.control(data_set[2],RPS[2]/Limit_angularvelocity,timer);
+    control[3]=pid_l.control(data_set[3],RPS[3]/Limit_angularvelocity,timer);
+    for(i=0;i<4;i++){
+        data_spd[i]=data_spd[i]+control[i];
+        if(data_spd[i]>1.0){
+            data_spd[i]=1.0;
+        }
+    }
+    up_motor.speed(data_spd[0]);
+    down_motor.speed(data_spd[1]);
+    right_motor.speed(data_spd[2]);
+    left_motor.speed(data_spd[3]);
+}
+
+void motorstop(){
+    up_motor.speed(0);
+    down_motor.speed(0);
+    right_motor.speed(0);
+    left_motor.speed(0);
+}
+
+void Arduino1(){
+    int val = 0;
+    val = !i2c.read(addra,p,3);
+    LED=val;
+}
+
+void Arduino2(int d){
+    wait_ms(10);
+    i2c.start();
+    i2c.write(addrb);
+    i2c.write(d);
+    i2c.stop();
+}
+
+void Linetrace(){
+    double dis;
+  
+    if(GREEN<cal_g[2] || GREEN<cal_g[3] || GREEN<cal_g[4] || GREEN<cal_g[5]){
+        //2=right up, 3=left up, 4=right down, 5=left down
+
+        if((GREEN<cal_g[2] || GREEN<cal_g[5]) && GREEN>cal_g[3] && GREEN>cal_g[4]){
+            motor_con.setspeed(0,0,5*s);//right up,left down nara hidarikaiten
+            motor_con.getspeed(data_set);
+            if(now!=1){
+                init();
+                now=1;
+            }
+            run();
+        }
+
+        if((GREEN<cal_g[3] || GREEN<cal_g[4]) && GREEN>cal_g[2] && GREEN>cal_g[5]){
+            motor_con.setspeed(0,0,-5*s);//right down,left up nara migikaiten
+            motor_con.getspeed(data_set);
+            if(now!=2){
+                init();
+                now=2;
+            }
+            run();
+        }
+
+        if(GREEN<cal_g[2] && GREEN<cal_g[3]){
+            motor_con.setspeed(0,-0.3,0);//ue 2ko nara ueidou
+            motor_con.getspeed(data_set);
+            if(now!=3){
+                init();
+                now=3;
+            }
+            run();
+        }
+
+        if(GREEN<cal_g[4] && GREEN<cal_g[5]){
+            motor_con.setspeed(0,0.3,0);//sita 2ko nara sitaidou
+            motor_con.getspeed(data_set);
+            if(now!=4){
+                init();
+                now=4;
+            }
+            run();
+        }
+    }
+    Arduino1();
+    dis=p[stage];
+    if(GREEN>cal_g[2] && GREEN>cal_g[3] && GREEN>cal_g[4] && GREEN>cal_g[5]){
+        if(dis <= 20){
+            motor_con.setspeed(-0.3*s,0,0);
+            motor_con.getspeed(data_set);
+            if(now!=5){
+                init();
+                now=5;
+            }
+            run();
+        }
+        else{
+            motor_con.setspeed(-0.5*s,0,0);
+            motor_con.getspeed(data_set);
+            if(now!=6){
+                init();
+                now=6;
+            }
+            run();
+        }//kentisiteinai nara migiidou
+    }
+}
+
+void getRGB(){
+    int count;
+    
+    for(count=1;count<=5;count++){
+        mux.select(count);
+        
+        ord[0] = 0x00;
+        ord[1] = 0x89;
+        Val = !i2c.write(address&0xFE, ord, 2, true);
+
+        ord[0] = 0x00;
+        ord[1] = 0x09;
+        Val = !i2c.write(address&0xFE, ord, 2);
+
+        wait_ms(5.6*4);
+
+        ord[0] = 0x03;
+        Val = !i2c.write(address&0xFE, ord, 1, true);
+        Val = !i2c.read(address|0x01, data, 8, true);
+
+        red[count]    =(uint8_t)(data[0])<<8 | (uint8_t)(data[1]);
+        green[count]  =(uint8_t)(data[2])<<8 | (uint8_t)(data[3]);
+        blue[count]   =(uint8_t)(data[4])<<8 | (uint8_t)(data[5]);
+        IR        =(uint8_t)(data[6])<<8 | (uint8_t)(data[7]);
+    }
+    return;
+}
+
+void Adjustment(){
+    /*int type;
+    type = 0;*/
+    //if(Linecount_str<=4){
+        motor_con.setspeed(0,0.3,0);
+        motor_con.getspeed(data_set);
+        init();
+        while(Situation != 6){
+            run();
+            if(Situation == 3 && GREEN*-1>cal_g[2]*-1){//right up
+                Situation = 4;
+            }
+            if(Situation == 3 && GREEN*-1>cal_g[3]*-1){//left up
+                Situation = 5 ;
+            }
+            if(Situation == 4 && GREEN*-1>cal_g[3]*-1){//right up
+                Situation = 6;
+            }
+            if(Situation == 5 && GREEN*-1>cal_g[2]*-1){//left up
+                Situation = 6 ;
+            }
+            /*if(type == 3 && GREEN<cal_g[2]*-1 && GREEN<cal_g[3]*-1){
+                type = 4;
+            }*/
+      /*type=1 right up only
+      　type=2 left up only
+      　type=3 both
+      　type=4 pass　　　    */
+        }
+        motor_con.stop();
+        motorstop();   
+    //}
+}
+
+void Injection(int i){
+    double nowdis=0;
+    Arduino1();
+    nowdis=p[2];
+    while(nowdis>4){
+        if(i%2==0){
+            INa=1;
+            INb=0;
+        }
+        if(i%2==1){
+            INa=0;
+            INb=1;
+        }
+        Arduino1();
+        nowdis=p[2];
+    }
+    INa=0;
+    INb=0;
+    PSB=0;
+    wait(1);
+    Arduino2(10);
+    wait(2);
+    Arduino2(15);
+    wait(2);
+}   
+
+void Cal_X(){
+    double rpms_vx=0,vx=0;
+    rpms_vx=-1*qei_u.getRPMS()+qei_d.getRPMS();
+    vx=rpms_vx*RADIUS;
+    x+=vx*qei_r.a;
+}
+
+void Cal_Y(){
+    double rpms_vy=0,vy=0;
+    rpms_vy=-1*qei_l.getRPMS()+qei_r.getRPMS();
+    vy=rpms_vy*RADIUS;
+    y+=vy*qei_d.a;
+}
+
+void jud(){
+    double cal[2][5];
+    double ans[2];
+    int i;
+    for(i=0;i<5;i++){
+        Arduino1();
+        if(p[0]==0){
+            p[0]=250;
+        }
+        if(p[1]==0){
+            p[1]=250;
+        }
+        cal[0][i]=p[0];
+        cal[1][i]=p[1];
+    }
+    for(i=0;i<5;i++){
+        ans[0]+=cal[0][i];
+        ans[1]+=cal[1][i];
+    }
+    ans[0]=ans[0]/5;
+    ans[1]=ans[1]/5;
+    if(ans[0]<ans[1]){
+        stage=0;
+        s=1;
+    }
+    if(ans[0]>ans[1]){
+        stage=1;
+        s=-1;
+    }
+}
\ No newline at end of file