Ryu Kaz
/
767zi
t6est
peripheral.cpp
- Committer:
- kazuryu
- Date:
- 2019-09-24
- Revision:
- 0:3dc012104243
- Child:
- 3:8b22783f6bf1
File content as of revision 0:3dc012104243:
#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); Serial peri_serial(USBTX,USBRX); int a= 100; 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(seven)peri_which = 0; else if(!seven && six)peri_which = 1; else if(!seven && !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); } 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 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); }