Diff: peripheral.cpp

Revision:

0:3dc012104243

Child:

3:8b22783f6bf1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/peripheral.cpp	Tue Sep 24 13:35:31 2019 +0000
@@ -0,0 +1,119 @@
+
+
+#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);
+    }
\ No newline at end of file