File content as of revision 4:9ba47e5db1e2:

#include "peripheral.h"
#include "mbed.h"

#define UP 0
#define BACK 1
#define RIGHT 2
#define LEFT 3
#define RUP 4
#define LUP 5
#define RBACK 6
#define LBACK 7
#define RROLL 8
#define LROLL 9

DigitalIn DD7(D4);//D7 -> D4
DigitalIn DD6(D6);
DigitalIn DD5(D5);
DigitalIn Dketer(PE_7);

DigitalOut trig(PF_5);

Serial peri_serial(USBTX,USBRX);

int a= 150;

int color = 0;
int peri_which = 0;
int roboy = 750;
int robox = 750;
int points_blue[][3] = {
    {UP,7100-roboy-a,0},
    {LEFT,3800-a,1},
    {RIGHT,3800-a,0},
    {BACK,7100-roboy-a,0},
    {UP,5100-roboy-a,0},
    {LEFT,3800-a,-1},
    {RIGHT,3800-a,0},
    {BACK,5100-roboy-a,0}
    };
//1:until[3] 2:from[4] to[7]
int points_orange[][3] = {
    {UP,7100-roboy-a,0},
    {RIGHT,3800-a,1},
    {LEFT,3800-a,0},
    {BACK,7100-roboy-a,0},
    {UP,5100-roboy-a,0},
    {RIGHT,3800-a,-1},
    {LEFT,3800-a,0},
    {BACK,5100-roboy-a,0}
    };

void setup_peri(){
    DD7.mode(PullDown);
    DD6.mode(PullDown);
    DD5.mode(PullDown);
    Dketer.mode(PullDown);
    }

int read_peri(){
    int seven = DD7.read();
    int six = DD6.read();
    if(six)peri_which = 1;
    else if(!six)peri_which = 2;
    color = DD5.read();
    peri_serial.printf("%d\n",color);
    if(!peri_which)return(8);
    else if(peri_which)return(4);
    else if(peri_which == 2)return(4);
    else return(8);
    
}
int getPeriwhich(){
    if(peri_which == 1)return(0);
    else return(1);//towel
    }
void changeColor(int *p){
    if(color == 0){//blue
        inarray(&points_blue[0][0],p,peri_which);
    }else if(color == 1){//orange
        inarray(&points_orange[0][0],p,peri_which);
    }
}
int getColor(){
    return(color);
    }
int read_keter(){
    int a = Dketer.read();
    if(a)return(1);
    else return(0);
    }
    
void inarray(int *points,int *p,int which){
    if(which == 0){
    for(int i =0;i<24;i++){
        *(p+i) = *(points+i);
        }
    }else if(which == 1){
        for(int i = 0;i<12;i++){ //< 4*2
           *(p+i) = *(points+i);
            }
    }else if(which == 2){
        for(int i = 12;i<24;i++){// 4*2 < i < 8*2
            *(p+i-12) = *(points+i);
            }
        }
    }
    
int getRobox(){
    return(robox);
    }

int humminging(){//putc
    int x1_p,x2_p,x3_p,x4_p,p1_p,p2_p,p3_p,pputs;
    x1_p = color;
    x2_p = 0;
    x3_p = peri_which & 0b01;
    x4_p = peri_which >> 1;
    p1_p = x1_p ^ x3_p ^ x4_p;
    p2_p = x1_p ^ x2_p ^ x4_p;
    p3_p = x1_p ^ x2_p ^ x3_p;
    pputs = x1_p | x2_p << 1 | x3_p << 2| x4_p << 3 | p1_p << 4 | p2_p << 5 | p3_p << 6;
    return(pputs);
    }