Ryu Kaz / Mbed OS 767zi

t6est

Dependencies:   Pulse

peripheral.cpp@0:3dc012104243, 2019-09-24 (annotated)

Committer:
kazuryu
Date:
Tue Sep 24 13:35:31 2019 +0000
Revision:
0:3dc012104243
Child:
3:8b22783f6bf1
test;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kazuryu 0:3dc012104243 1
kazuryu 0:3dc012104243 2
kazuryu 0:3dc012104243 3 #include "peripheral.h"
kazuryu 0:3dc012104243 4 #include "mbed.h"
kazuryu 0:3dc012104243 5
kazuryu 0:3dc012104243 6 #define UP 0
kazuryu 0:3dc012104243 7 #define BACK 1
kazuryu 0:3dc012104243 8 #define RIGHT 2
kazuryu 0:3dc012104243 9 #define LEFT 3
kazuryu 0:3dc012104243 10 #define RUP 4
kazuryu 0:3dc012104243 11 #define LUP 5
kazuryu 0:3dc012104243 12 #define RBACK 6
kazuryu 0:3dc012104243 13 #define LBACK 7
kazuryu 0:3dc012104243 14 #define RROLL 8
kazuryu 0:3dc012104243 15 #define LROLL 9
kazuryu 0:3dc012104243 16
kazuryu 0:3dc012104243 17 DigitalIn DD7(D4);//D7 -> D4
kazuryu 0:3dc012104243 18 DigitalIn DD6(D6);
kazuryu 0:3dc012104243 19 DigitalIn DD5(D5);
kazuryu 0:3dc012104243 20 DigitalIn Dketer(PE_7);
kazuryu 0:3dc012104243 21
kazuryu 0:3dc012104243 22 Serial peri_serial(USBTX,USBRX);
kazuryu 0:3dc012104243 23
kazuryu 0:3dc012104243 24 int a= 100;
kazuryu 0:3dc012104243 25
kazuryu 0:3dc012104243 26 int color = 0;
kazuryu 0:3dc012104243 27 int peri_which = 0;
kazuryu 0:3dc012104243 28 int roboy = 750;
kazuryu 0:3dc012104243 29 int robox = 750;
kazuryu 0:3dc012104243 30 int points_blue[][3] = {
kazuryu 0:3dc012104243 31 {UP,7100-roboy-a,0},
kazuryu 0:3dc012104243 32 {LEFT,3800-a,1},
kazuryu 0:3dc012104243 33 {RIGHT,3800-a,0},
kazuryu 0:3dc012104243 34 {BACK,7100-roboy-a,0},
kazuryu 0:3dc012104243 35 {UP,5100-roboy-a,0},
kazuryu 0:3dc012104243 36 {LEFT,3800-a,-1},
kazuryu 0:3dc012104243 37 {RIGHT,3800-a,0},
kazuryu 0:3dc012104243 38 {BACK,5100-roboy-a,0}
kazuryu 0:3dc012104243 39 };
kazuryu 0:3dc012104243 40 //1:until[3] 2:from[4] to[7]
kazuryu 0:3dc012104243 41 int points_orange[][3] = {
kazuryu 0:3dc012104243 42 {UP,7100-roboy-a,0},
kazuryu 0:3dc012104243 43 {RIGHT,3800-a,1},
kazuryu 0:3dc012104243 44 {LEFT,3800-a,0},
kazuryu 0:3dc012104243 45 {BACK,7100-roboy-a,0},
kazuryu 0:3dc012104243 46 {UP,5100-roboy-a,0},
kazuryu 0:3dc012104243 47 {RIGHT,3800-a,-1},
kazuryu 0:3dc012104243 48 {LEFT,3800-a,0},
kazuryu 0:3dc012104243 49 {BACK,5100-roboy-a,0}
kazuryu 0:3dc012104243 50 };
kazuryu 0:3dc012104243 51
kazuryu 0:3dc012104243 52 void setup_peri(){
kazuryu 0:3dc012104243 53 DD7.mode(PullDown);
kazuryu 0:3dc012104243 54 DD6.mode(PullDown);
kazuryu 0:3dc012104243 55 DD5.mode(PullDown);
kazuryu 0:3dc012104243 56 Dketer.mode(PullDown);
kazuryu 0:3dc012104243 57 }
kazuryu 0:3dc012104243 58
kazuryu 0:3dc012104243 59 int read_peri(){
kazuryu 0:3dc012104243 60 int seven = DD7.read();
kazuryu 0:3dc012104243 61 int six = DD6.read();
kazuryu 0:3dc012104243 62 if(seven)peri_which = 0;
kazuryu 0:3dc012104243 63 else if(!seven && six)peri_which = 1;
kazuryu 0:3dc012104243 64 else if(!seven && !six)peri_which = 2;
kazuryu 0:3dc012104243 65 color = DD5.read();
kazuryu 0:3dc012104243 66 peri_serial.printf("%d\n",color);
kazuryu 0:3dc012104243 67 if(!peri_which)return(8);
kazuryu 0:3dc012104243 68 else if(peri_which)return(4);
kazuryu 0:3dc012104243 69 else if(peri_which == 2)return(4);
kazuryu 0:3dc012104243 70 else return(8);
kazuryu 0:3dc012104243 71
kazuryu 0:3dc012104243 72 }
kazuryu 0:3dc012104243 73
kazuryu 0:3dc012104243 74 void changeColor(int *p){
kazuryu 0:3dc012104243 75 if(color == 0){//blue
kazuryu 0:3dc012104243 76 inarray(&points_blue[0][0],p,peri_which);
kazuryu 0:3dc012104243 77 }else if(color == 1){//orange
kazuryu 0:3dc012104243 78 inarray(&points_orange[0][0],p,peri_which);
kazuryu 0:3dc012104243 79 }
kazuryu 0:3dc012104243 80 }
kazuryu 0:3dc012104243 81
kazuryu 0:3dc012104243 82 int read_keter(){
kazuryu 0:3dc012104243 83 int a = Dketer.read();
kazuryu 0:3dc012104243 84 if(a)return(1);
kazuryu 0:3dc012104243 85 else return(0);
kazuryu 0:3dc012104243 86 }
kazuryu 0:3dc012104243 87
kazuryu 0:3dc012104243 88 void inarray(int *points,int *p,int which){
kazuryu 0:3dc012104243 89 if(which == 0){
kazuryu 0:3dc012104243 90 for(int i =0;i<24;i++){
kazuryu 0:3dc012104243 91 *(p+i) = *(points+i);
kazuryu 0:3dc012104243 92 }
kazuryu 0:3dc012104243 93 }else if(which == 1){
kazuryu 0:3dc012104243 94 for(int i = 0;i<12;i++){ //< 4*2
kazuryu 0:3dc012104243 95 *(p+i) = *(points+i);
kazuryu 0:3dc012104243 96 }
kazuryu 0:3dc012104243 97 }else if(which == 2){
kazuryu 0:3dc012104243 98 for(int i = 12;i<24;i++){// 4*2 < i < 8*2
kazuryu 0:3dc012104243 99 *(p+i-12) = *(points+i);
kazuryu 0:3dc012104243 100 }
kazuryu 0:3dc012104243 101 }
kazuryu 0:3dc012104243 102 }
kazuryu 0:3dc012104243 103
kazuryu 0:3dc012104243 104 int getRobox(){
kazuryu 0:3dc012104243 105 return(robox);
kazuryu 0:3dc012104243 106 }
kazuryu 0:3dc012104243 107
kazuryu 0:3dc012104243 108 int humminging(){//putc
kazuryu 0:3dc012104243 109 int x1_p,x2_p,x3_p,x4_p,p1_p,p2_p,p3_p,pputs;
kazuryu 0:3dc012104243 110 x1_p = color;
kazuryu 0:3dc012104243 111 x2_p = 0;
kazuryu 0:3dc012104243 112 x3_p = peri_which & 0b01;
kazuryu 0:3dc012104243 113 x4_p = peri_which >> 1;
kazuryu 0:3dc012104243 114 p1_p = x1_p ^ x3_p ^ x4_p;
kazuryu 0:3dc012104243 115 p2_p = x1_p ^ x2_p ^ x4_p;
kazuryu 0:3dc012104243 116 p3_p = x1_p ^ x2_p ^ x3_p;
kazuryu 0:3dc012104243 117 pputs = x1_p | x2_p << 1 | x3_p << 2| x4_p << 3 | p1_p << 4 | p2_p << 5 | p3_p << 6;
kazuryu 0:3dc012104243 118 return(pputs);
kazuryu 0:3dc012104243 119 }