Kiko Ishimoto
い椅子ね〜
Revision 0:4a1a924f704e, committed 2017-10-25
- Comitter:
- kikoaac
- Date:
- Wed Oct 25 10:58:54 2017 +0000
- Commit message:
- ??
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MyLib.lib Wed Oct 25 10:58:54 2017 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/kikoaac/code/MyLib3/#d50a0e730654
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Oct 25 10:58:54 2017 +0000
@@ -0,0 +1,169 @@
+#include "mbed.h"
+char *tmp[2];
+char *SenseTmp[2];
+#include "Nunchuck.h"
+#include "defines.h"
+#include "param_motion.h"
+Serial dev(D1,D0);
+Serial sbdbt(D13,D12);
+#include "methods.h"
+
+DigitalOut debugLed1(D6);
+DigitalOut debugLed2(D7);
+bool send = false;
+
+//uint16_t MinimumRangeL[4] = {19000,35000,35600,21000};
+//uint16_t MaxmumRangeL[4] = {49000,57000,43000,42000 };
+bool ReverseL[4] = {true,false,true,true};
+bool ReverseR[4] = {false,false,false,true};
+Timer timer;
+void RX(){
+ if(dev.getc() == '0'){
+ timer.reset();
+ debugLed1 = true;
+ for(int i = 1 ; i < dataNum;i++){
+ RXData[i] = dev.getc();
+ }
+ //if(DEBUG && !DEBUG_R)sbdbt.printf("L:");
+ for(int i = 0 ;i < 4 ; i++){
+ floatInByte in;//( (uint16_t)tmp[R][5 + i*2] << 8 ) | (uint16_t)tmp[R][4 + i*2];
+ in.c[0] = RXData[4 + i*2];//tmp[R][5 + i*2];
+ in.c[1] = RXData[5 + i*2];//tmp[R][4 + i*2];
+ uint16_t in_ = ArmSense2[i].read_u16(float(in.si)/0xffff);
+ uint16_t intt = map(in_,MinimumRangeL[i],MaxmumRangeL[i],0,65535);
+ intt = ReverseL[i] == true ? 0xffff - intt : intt;
+ floatInByte intt_;
+ intt_.si = intt;
+ //if(DEBUG && !DEBUG_R)sbdbt.printf(" %5d ",intt);
+ //uint16_t intt = map(in_,13107,52428,0,65535);
+ ArmInputSense[L][i].read_u16(float(intt_.si)/0xffff);
+ RXData[4 + i*2] = intt_.c[0];//uint8_t(intt>>8);//マスター片腕
+ RXData[5 + i*2] = intt_.c[1];//uint8_t(intt&0xff); //マスター片腕
+ }//if(DEBUG && !DEBUG_R)sbdbt.printf("\n");
+
+ timer.reset();
+ send = true;
+ }
+}
+void print(int N,char RXdata[12]){
+ floatInByte data;
+ for(int i = 0 ; i < 2 ; i ++){
+ data.c[i] = RXdata[N+i];
+ }
+ sbdbt.printf("%d ",data.si);
+}
+int id = 0;
+int main() {
+
+ dev.baud(115200);
+
+ sbdbt.baud(115200);
+ for(int i = 0 ; i < 2; i++)
+ {
+ tmp[i] = new char[dataNum];
+ }
+ debugLed1 = true;
+ for(int i = 0 ; i < 50 ; i ++ ){
+ debugLed2 = !debugLed2;
+ wait(0.1);
+ }
+ dev.attach(RX, Serial::RxIrq);
+ dev.putc('L');
+ timer.start();
+ motionTimer.start();
+ while(1) {
+ //送信データ格納
+
+ ctrl.getdata();
+ tmp[R][0] = 'H';
+ tmp[R][1] = ctrl.analogx();//ヌンチャクアナログX
+ tmp[R][2] = ctrl.analogy();//ヌンチャクアナログy
+ tmp[R][3] = (ctrl.buttonc()<<1)|(ctrl.buttonz());//ヌンチャクzボタンとCボタン
+ for(int i = 0 ;i < 4 ; i++){
+ floatInByte intt_;
+ //if(i==0 && DEBUG && DEBUG_R)sbdbt.printf("R:");
+ uint16_t in = ArmSense[i].read_u16();
+ //if(DEBUG && DEBUG_R)sbdbt.printf(" %5d ",intt);
+ uint16_t intt = map(in, MinimumRangeR[i],MaxmumRangeR[i],0,65535);
+ intt = ReverseR[i] == true ? 0xffff - intt : intt;
+ intt_.si = intt;
+ ArmInputSense[R][i].read_u16(float(intt_.si)/0xffff);
+ //uint16_t intt = map(in_,13107,52428,0,65535);
+ tmp[R][4 + i*2] = intt_.c[0];//uint8_t(intt>>8);//マスター片腕
+ tmp[R][5 + i*2] = intt_.c[1];//uint8_t(intt&0xff); //マスター片腕
+ }
+ tmp[L] = (char*)RXData;
+ signed char x1 = map8(tmp[R][1],-90,90,-100,100)+1;
+ signed char y1 = map8(tmp[R][2],-90,90,-100,100)-7;
+ signed char x2 = map8(tmp[L][1],-90,90,-100,100)+3;
+ signed char y2 = map8(tmp[L][2],-90,90,-100,100)-10;
+ //sbdbt.printf("%d %d %d %d ",x1,y1,x2,y2);
+ sbdbt.printf("sita %d ",impulse(float(y2)/100.0F));
+ char** SerialData = ;
+ bool txFlag = true;
+ switch(int i = MotionSense()){
+ case NOTINPUT:
+ //tmp[L] = (char*)RXData;
+ //SerialData = tmp;
+ break;
+ case LOADMOTION:
+ //sbdbt.printf("LOADMOTION");
+ //tmp[L] = (char*)RXData;
+ //SerialData = tmp;
+ break;
+ case TIMEOUT:
+ //sbdbt.printf("TIMEOUT");
+ //tmp[L] = (char*)RXData;
+ //SerialData = tmp;
+ break;
+ default:
+ id = i;
+ sbdbt.printf("GET %d ",i);
+ txFlag = false;
+ break;
+ }
+ //if(playMotionFlag == true){
+ playMotion(0,(uint8_t**)SerialData);
+ //}
+ //if(DEBUG && DEBUG_R)sbdbt.printf("\n");
+ sbdbt.printf("\n");
+ //送信データを送る
+ //SerialData = tmp;
+ if(send == true || txFlag == true){
+ for(int j = 0; j < 2 ; j++){
+ for(int i = 0 ; i < dataNum ; i++){
+ //if(!DEBUG)sbdbt.printf("%3d ",SerialData[j][i]);
+ //if(!DEBUG)sbdbt.putc(SerialData[j][i]);
+ }
+ }
+ //sbdbt.printf("R:");
+ //for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[R]);
+ //sbdbt.printf("L:");
+ //for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[L]);
+ for(int j = 0; j < 2 ; j++){
+ for(int i = 0 ; i < dataNum ; i++){
+ sbdbt.printf("%3d ",SerialData[j][i]);
+ //if(!DEBUG)sbdbt.putc(SerialData[j][i]);
+ }
+ }
+ if(!DEBUG)sbdbt.printf("\n");
+ }
+ dev.putc('L');
+ send = false;
+ while(timer.read_ms() >= 2000){
+ debugLed2 = true;
+ waitTime(0.1);
+ debugLed2 = false;
+ waitTime(0.1);
+ }
+ debugLed1 = false;
+ waitTime(0.01);
+ #if DEBUG
+ int RL = R;
+ //if(DEBUG_R){sbdbt.printf("R:");RL = R;}
+ //if(!DEBUG_R){sbdbt.printf("L:");RL = L;}
+ //for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[RL]);
+ //sbdbt.printf("\n");
+ #endif
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Oct 25 10:58:54 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/675da3299148 \ No newline at end of file