Ryu Kaz / Mbed OS 767zi

t6est

Dependencies:   Pulse

peripheral.cpp@4:9ba47e5db1e2, 2019-10-05 (annotated)

Committer:
kazuryu
Date:
Sat Oct 05 01:41:37 2019 +0000
Revision:
4:9ba47e5db1e2
Parent:
3:8b22783f6bf1 
B

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 4:9ba47e5db1e2 22 DigitalOut trig(PF_5);
kazuryu 4:9ba47e5db1e2 23
kazuryu 0:3dc012104243 24 Serial peri_serial(USBTX,USBRX);
kazuryu 0:3dc012104243 25
kazuryu 3:8b22783f6bf1 26 int a= 150;
kazuryu 0:3dc012104243 27
kazuryu 0:3dc012104243 28 int color = 0;
kazuryu 0:3dc012104243 29 int peri_which = 0;
kazuryu 0:3dc012104243 30 int roboy = 750;
kazuryu 0:3dc012104243 31 int robox = 750;
kazuryu 0:3dc012104243 32 int points_blue[][3] = {
kazuryu 0:3dc012104243 33 {UP,7100-roboy-a,0},
kazuryu 0:3dc012104243 34 {LEFT,3800-a,1},
kazuryu 0:3dc012104243 35 {RIGHT,3800-a,0},
kazuryu 0:3dc012104243 36 {BACK,7100-roboy-a,0},
kazuryu 0:3dc012104243 37 {UP,5100-roboy-a,0},
kazuryu 0:3dc012104243 38 {LEFT,3800-a,-1},
kazuryu 0:3dc012104243 39 {RIGHT,3800-a,0},
kazuryu 0:3dc012104243 40 {BACK,5100-roboy-a,0}
kazuryu 0:3dc012104243 41 };
kazuryu 0:3dc012104243 42 //1:until[3] 2:from[4] to[7]
kazuryu 0:3dc012104243 43 int points_orange[][3] = {
kazuryu 0:3dc012104243 44 {UP,7100-roboy-a,0},
kazuryu 0:3dc012104243 45 {RIGHT,3800-a,1},
kazuryu 0:3dc012104243 46 {LEFT,3800-a,0},
kazuryu 0:3dc012104243 47 {BACK,7100-roboy-a,0},
kazuryu 0:3dc012104243 48 {UP,5100-roboy-a,0},
kazuryu 0:3dc012104243 49 {RIGHT,3800-a,-1},
kazuryu 0:3dc012104243 50 {LEFT,3800-a,0},
kazuryu 0:3dc012104243 51 {BACK,5100-roboy-a,0}
kazuryu 0:3dc012104243 52 };
kazuryu 0:3dc012104243 53
kazuryu 0:3dc012104243 54 void setup_peri(){
kazuryu 0:3dc012104243 55 DD7.mode(PullDown);
kazuryu 0:3dc012104243 56 DD6.mode(PullDown);
kazuryu 0:3dc012104243 57 DD5.mode(PullDown);
kazuryu 0:3dc012104243 58 Dketer.mode(PullDown);
kazuryu 0:3dc012104243 59 }
kazuryu 0:3dc012104243 60
kazuryu 0:3dc012104243 61 int read_peri(){
kazuryu 0:3dc012104243 62 int seven = DD7.read();
kazuryu 0:3dc012104243 63 int six = DD6.read();
kazuryu 3:8b22783f6bf1 64 if(six)peri_which = 1;
kazuryu 3:8b22783f6bf1 65 else if(!six)peri_which = 2;
kazuryu 0:3dc012104243 66 color = DD5.read();
kazuryu 0:3dc012104243 67 peri_serial.printf("%d\n",color);
kazuryu 0:3dc012104243 68 if(!peri_which)return(8);
kazuryu 0:3dc012104243 69 else if(peri_which)return(4);
kazuryu 0:3dc012104243 70 else if(peri_which == 2)return(4);
kazuryu 0:3dc012104243 71 else return(8);
kazuryu 0:3dc012104243 72
kazuryu 0:3dc012104243 73 }
kazuryu 3:8b22783f6bf1 74 int getPeriwhich(){
kazuryu 3:8b22783f6bf1 75 if(peri_which == 1)return(0);
kazuryu 3:8b22783f6bf1 76 else return(1);//towel
kazuryu 3:8b22783f6bf1 77 }
kazuryu 0:3dc012104243 78 void changeColor(int *p){
kazuryu 0:3dc012104243 79 if(color == 0){//blue
kazuryu 0:3dc012104243 80 inarray(&points_blue[0][0],p,peri_which);
kazuryu 0:3dc012104243 81 }else if(color == 1){//orange
kazuryu 0:3dc012104243 82 inarray(&points_orange[0][0],p,peri_which);
kazuryu 0:3dc012104243 83 }
kazuryu 0:3dc012104243 84 }
kazuryu 3:8b22783f6bf1 85 int getColor(){
kazuryu 3:8b22783f6bf1 86 return(color);
kazuryu 3:8b22783f6bf1 87 }
kazuryu 0:3dc012104243 88 int read_keter(){
kazuryu 0:3dc012104243 89 int a = Dketer.read();
kazuryu 0:3dc012104243 90 if(a)return(1);
kazuryu 0:3dc012104243 91 else return(0);
kazuryu 0:3dc012104243 92 }
kazuryu 0:3dc012104243 93
kazuryu 0:3dc012104243 94 void inarray(int *points,int *p,int which){
kazuryu 0:3dc012104243 95 if(which == 0){
kazuryu 0:3dc012104243 96 for(int i =0;i<24;i++){
kazuryu 0:3dc012104243 97 *(p+i) = *(points+i);
kazuryu 0:3dc012104243 98 }
kazuryu 0:3dc012104243 99 }else if(which == 1){
kazuryu 0:3dc012104243 100 for(int i = 0;i<12;i++){ //< 4*2
kazuryu 0:3dc012104243 101 *(p+i) = *(points+i);
kazuryu 0:3dc012104243 102 }
kazuryu 0:3dc012104243 103 }else if(which == 2){
kazuryu 0:3dc012104243 104 for(int i = 12;i<24;i++){// 4*2 < i < 8*2
kazuryu 0:3dc012104243 105 *(p+i-12) = *(points+i);
kazuryu 0:3dc012104243 106 }
kazuryu 0:3dc012104243 107 }
kazuryu 0:3dc012104243 108 }
kazuryu 0:3dc012104243 109
kazuryu 0:3dc012104243 110 int getRobox(){
kazuryu 0:3dc012104243 111 return(robox);
kazuryu 0:3dc012104243 112 }
kazuryu 0:3dc012104243 113
kazuryu 0:3dc012104243 114 int humminging(){//putc
kazuryu 0:3dc012104243 115 int x1_p,x2_p,x3_p,x4_p,p1_p,p2_p,p3_p,pputs;
kazuryu 0:3dc012104243 116 x1_p = color;
kazuryu 0:3dc012104243 117 x2_p = 0;
kazuryu 0:3dc012104243 118 x3_p = peri_which & 0b01;
kazuryu 0:3dc012104243 119 x4_p = peri_which >> 1;
kazuryu 0:3dc012104243 120 p1_p = x1_p ^ x3_p ^ x4_p;
kazuryu 0:3dc012104243 121 p2_p = x1_p ^ x2_p ^ x4_p;
kazuryu 0:3dc012104243 122 p3_p = x1_p ^ x2_p ^ x3_p;
kazuryu 0:3dc012104243 123 pputs = x1_p | x2_p << 1 | x3_p << 2| x4_p << 3 | p1_p << 4 | p2_p << 5 | p3_p << 6;
kazuryu 0:3dc012104243 124 return(pputs);
kazuryu 0:3dc012104243 125 }