miki sumito / Mbed 2 deprecated four_wheel_omni

四輪オムニの足回りを動かすプログラム

Dependencies:   mbed

SBDBT/SBDBT.cpp@1:cb4a6d5c77c8, 2022-10-08 (annotated)

Committer:
m_smt
Date:
Sat Oct 08 11:14:39 2022 +0000
Revision:
1:cb4a6d5c77c8
move four wheel omni

Who changed what in which revision?

UserRevisionLine numberNew contents of line
m_smt 1:cb4a6d5c77c8 1 #include "SBDBT.hpp"
m_smt 1:cb4a6d5c77c8 2 #include "mbed.h"
m_smt 1:cb4a6d5c77c8 3
m_smt 1:cb4a6d5c77c8 4 Serial pc(USBTX,USBRX);
m_smt 1:cb4a6d5c77c8 5
m_smt 1:cb4a6d5c77c8 6 sbdbt::sbdbt(PinName TX, PinName RX){
m_smt 1:cb4a6d5c77c8 7 i = 0;
m_smt 1:cb4a6d5c77c8 8 pos = 0;
m_smt 1:cb4a6d5c77c8 9 val = 0x80;
m_smt 1:cb4a6d5c77c8 10 len = 10;
m_smt 1:cb4a6d5c77c8 11 data = new char[len];
m_smt 1:cb4a6d5c77c8 12 PS3_Data = new char[len];
m_smt 1:cb4a6d5c77c8 13 dev = new Serial(TX, RX);
m_smt 1:cb4a6d5c77c8 14 dev -> baud(9600);
m_smt 1:cb4a6d5c77c8 15 dev -> attach(callback(this,&sbdbt::get),Serial::RxIrq);
m_smt 1:cb4a6d5c77c8 16 }
m_smt 1:cb4a6d5c77c8 17
m_smt 1:cb4a6d5c77c8 18 sbdbt::sbdbt(PinName TX, PinName RX, int baud){
m_smt 1:cb4a6d5c77c8 19 i = 0;
m_smt 1:cb4a6d5c77c8 20 pos = 0;
m_smt 1:cb4a6d5c77c8 21 val = 0x80;
m_smt 1:cb4a6d5c77c8 22 len = 10;
m_smt 1:cb4a6d5c77c8 23 data = new char[len];
m_smt 1:cb4a6d5c77c8 24 PS3_Data = new char[len];
m_smt 1:cb4a6d5c77c8 25 dev = new Serial(TX, RX);
m_smt 1:cb4a6d5c77c8 26 dev -> baud(9600);
m_smt 1:cb4a6d5c77c8 27 pc.baud(baud);
m_smt 1:cb4a6d5c77c8 28 dev -> attach(callback(this,&sbdbt::get),Serial::RxIrq);
m_smt 1:cb4a6d5c77c8 29 }
m_smt 1:cb4a6d5c77c8 30
m_smt 1:cb4a6d5c77c8 31 void sbdbt::get(){
m_smt 1:cb4a6d5c77c8 32 data[i] = dev -> getc();
m_smt 1:cb4a6d5c77c8 33 if(data[i] == val){
m_smt 1:cb4a6d5c77c8 34 if(i != pos){
m_smt 1:cb4a6d5c77c8 35 data[pos] = val;
m_smt 1:cb4a6d5c77c8 36 i = pos + 1;
m_smt 1:cb4a6d5c77c8 37 }
m_smt 1:cb4a6d5c77c8 38 else{
m_smt 1:cb4a6d5c77c8 39 i++;
m_smt 1:cb4a6d5c77c8 40 }
m_smt 1:cb4a6d5c77c8 41 }
m_smt 1:cb4a6d5c77c8 42 else{
m_smt 1:cb4a6d5c77c8 43 if(i == pos);
m_smt 1:cb4a6d5c77c8 44 else{
m_smt 1:cb4a6d5c77c8 45 i++;
m_smt 1:cb4a6d5c77c8 46 }
m_smt 1:cb4a6d5c77c8 47 }
m_smt 1:cb4a6d5c77c8 48 if(i >= len){
m_smt 1:cb4a6d5c77c8 49 PS3_Data[0] = data[0];
m_smt 1:cb4a6d5c77c8 50 PS3_Data[1] = data[1];
m_smt 1:cb4a6d5c77c8 51 PS3_Data[2] = data[2];
m_smt 1:cb4a6d5c77c8 52 PS3_Data[3] = data[3];
m_smt 1:cb4a6d5c77c8 53 PS3_Data[4] = data[4];
m_smt 1:cb4a6d5c77c8 54 PS3_Data[5] = data[5];
m_smt 1:cb4a6d5c77c8 55 PS3_Data[6] = data[6];
m_smt 1:cb4a6d5c77c8 56 PS3_Data[7] = data[7];
m_smt 1:cb4a6d5c77c8 57 PS3_Data[8] = data[8];
m_smt 1:cb4a6d5c77c8 58 PS3_Data[9] = data[9];
m_smt 1:cb4a6d5c77c8 59 i = 0;
m_smt 1:cb4a6d5c77c8 60 }
m_smt 1:cb4a6d5c77c8 61 }
m_smt 1:cb4a6d5c77c8 62
m_smt 1:cb4a6d5c77c8 63 void sbdbt::button_state(){pc.printf("UP: %d, DOW: %d, RIG: %d, LEF: %d, TRI: %d, CRO: %d, CIR: %d, SQU: %d, L1: %d, L2: %d, R1: %d, R2: %d, STA: %d, SEL: %d LSX: %3d, LSY: %3d, RSX: %3d, RY: %3d, L2An: %3d, R2An: %3d\n",up(), down(), right(), left(), tri(), cross(), circle(), square(), l1(), l2(), r1(), r2(), start(), select(), lsx(), lsy(), rsx(), rsy(), l2An(), r2An());}
m_smt 1:cb4a6d5c77c8 64
m_smt 1:cb4a6d5c77c8 65 void sbdbt::data_print(){pc.printf("%2x %2x %2x %2x %2x %2x %2x %2x %2x %2x\n",PS3_Data[0], PS3_Data[1], PS3_Data[2], PS3_Data[3], PS3_Data[4], PS3_Data[5], PS3_Data[6], PS3_Data[7], PS3_Data[8], PS3_Data[9]);}
m_smt 1:cb4a6d5c77c8 66
m_smt 1:cb4a6d5c77c8 67 bool sbdbt::neutral(){
m_smt 1:cb4a6d5c77c8 68 if(data[9] == 0) return 1;
m_smt 1:cb4a6d5c77c8 69 return 0;
m_smt 1:cb4a6d5c77c8 70 }
m_smt 1:cb4a6d5c77c8 71
m_smt 1:cb4a6d5c77c8 72 bool sbdbt::up(){
m_smt 1:cb4a6d5c77c8 73 if(data[2] & UP) return 1;
m_smt 1:cb4a6d5c77c8 74 return 0;
m_smt 1:cb4a6d5c77c8 75 }
m_smt 1:cb4a6d5c77c8 76
m_smt 1:cb4a6d5c77c8 77 bool sbdbt::down(){
m_smt 1:cb4a6d5c77c8 78 if(data[2] & DOWN) return 1;
m_smt 1:cb4a6d5c77c8 79 return 0;
m_smt 1:cb4a6d5c77c8 80 }
m_smt 1:cb4a6d5c77c8 81
m_smt 1:cb4a6d5c77c8 82 bool sbdbt::left(){
m_smt 1:cb4a6d5c77c8 83 if(data[2] & LEFT) return 1;
m_smt 1:cb4a6d5c77c8 84 return 0;
m_smt 1:cb4a6d5c77c8 85 }
m_smt 1:cb4a6d5c77c8 86
m_smt 1:cb4a6d5c77c8 87 bool sbdbt::right(){
m_smt 1:cb4a6d5c77c8 88 if(data[2] & RIGHT) return 1;
m_smt 1:cb4a6d5c77c8 89 return 0;
m_smt 1:cb4a6d5c77c8 90 }
m_smt 1:cb4a6d5c77c8 91
m_smt 1:cb4a6d5c77c8 92 bool sbdbt::tri(){
m_smt 1:cb4a6d5c77c8 93 if(data[2] & TRI) return 1;
m_smt 1:cb4a6d5c77c8 94 return 0;
m_smt 1:cb4a6d5c77c8 95 }
m_smt 1:cb4a6d5c77c8 96
m_smt 1:cb4a6d5c77c8 97 bool sbdbt::cross(){
m_smt 1:cb4a6d5c77c8 98 if(data[2] & CROSS) return 1;
m_smt 1:cb4a6d5c77c8 99 return 0;
m_smt 1:cb4a6d5c77c8 100 }
m_smt 1:cb4a6d5c77c8 101
m_smt 1:cb4a6d5c77c8 102 bool sbdbt::square(){
m_smt 1:cb4a6d5c77c8 103 if(data[2] & SQUARE) return 1;
m_smt 1:cb4a6d5c77c8 104 return 0;
m_smt 1:cb4a6d5c77c8 105 }
m_smt 1:cb4a6d5c77c8 106
m_smt 1:cb4a6d5c77c8 107 bool sbdbt::circle(){
m_smt 1:cb4a6d5c77c8 108 if(data[1] & CIRCLE) return 1;
m_smt 1:cb4a6d5c77c8 109 return 0;
m_smt 1:cb4a6d5c77c8 110 }
m_smt 1:cb4a6d5c77c8 111 bool sbdbt::l1(){
m_smt 1:cb4a6d5c77c8 112 if(data[1] & L1) return 1;
m_smt 1:cb4a6d5c77c8 113 return 0;
m_smt 1:cb4a6d5c77c8 114 }
m_smt 1:cb4a6d5c77c8 115
m_smt 1:cb4a6d5c77c8 116 bool sbdbt::l2(){
m_smt 1:cb4a6d5c77c8 117 if(data[1] & L2) return 1;
m_smt 1:cb4a6d5c77c8 118 return 0;
m_smt 1:cb4a6d5c77c8 119 }
m_smt 1:cb4a6d5c77c8 120
m_smt 1:cb4a6d5c77c8 121 bool sbdbt::r1(){
m_smt 1:cb4a6d5c77c8 122 if(data[1] & R1) return 1;
m_smt 1:cb4a6d5c77c8 123 return 0;
m_smt 1:cb4a6d5c77c8 124 }
m_smt 1:cb4a6d5c77c8 125
m_smt 1:cb4a6d5c77c8 126 bool sbdbt::r2(){
m_smt 1:cb4a6d5c77c8 127 if(data[1] & R2) return 1;
m_smt 1:cb4a6d5c77c8 128 return 0;
m_smt 1:cb4a6d5c77c8 129 }
m_smt 1:cb4a6d5c77c8 130
m_smt 1:cb4a6d5c77c8 131 bool sbdbt::start(){
m_smt 1:cb4a6d5c77c8 132 if(data[1] & START) return 1;
m_smt 1:cb4a6d5c77c8 133 return 0;
m_smt 1:cb4a6d5c77c8 134 }
m_smt 1:cb4a6d5c77c8 135
m_smt 1:cb4a6d5c77c8 136 bool sbdbt::select(){
m_smt 1:cb4a6d5c77c8 137 if(data[1] & SELECT) return 1;
m_smt 1:cb4a6d5c77c8 138 return 0;
m_smt 1:cb4a6d5c77c8 139 }
m_smt 1:cb4a6d5c77c8 140
m_smt 1:cb4a6d5c77c8 141 char sbdbt::lsx(){return data[3];}
m_smt 1:cb4a6d5c77c8 142
m_smt 1:cb4a6d5c77c8 143 char sbdbt::lsy(){return data[4];}
m_smt 1:cb4a6d5c77c8 144
m_smt 1:cb4a6d5c77c8 145 char sbdbt::rsx(){return data[5];}
m_smt 1:cb4a6d5c77c8 146
m_smt 1:cb4a6d5c77c8 147 char sbdbt::rsy(){return data[6];}
m_smt 1:cb4a6d5c77c8 148
m_smt 1:cb4a6d5c77c8 149 char sbdbt::l2An(){return data[7];}
m_smt 1:cb4a6d5c77c8 150
m_smt 1:cb4a6d5c77c8 151 char sbdbt::r2An(){return data[8];}
m_smt 1:cb4a6d5c77c8 152
m_smt 1:cb4a6d5c77c8 153 sbdbt::~sbdbt(){
m_smt 1:cb4a6d5c77c8 154 delete dev;
m_smt 1:cb4a6d5c77c8 155 delete data;
m_smt 1:cb4a6d5c77c8 156 delete PS3_Data;
m_smt 1:cb4a6d5c77c8 157 }