//DC_Servo_4 #include <stdlib.h> #include <ESP8266WiFi.h> const char* ssid = "SSID";//無線LANのSSIDを設定します。 const char* password = "password";//無線LANのpasswordを設定します。 const char* host = "192.168.11.2";//パソコンのIPアドレスを設定します。 const int httpPort = 13000;//TCPサーバのポート static String gSendText=""; int PS1B; int gPC_T,gPC_N; char inputString[50]; boolean stringComplete = false; // whether the string is complete int gi=0; int gCT=0;//カウンタ void setup() { Serial.begin(115200); pinMode(12, OUTPUT); pinMode(13, OUTPUT); pinMode(14, INPUT); pinMode(16, INPUT); gPC_T=0; gPC_N=0; PS1B=digitalRead(14); WiFi.begin(ssid, password);//無線LANに接続要求 while (WiFi.status() != WL_CONNECTED) {//接続完了まで待ちます。 delay(500); Serial.print("."); } Serial.println(""); Serial.println("WiFi connected"); } // the loop function runs over and over again forever void loop() { DC_Servo(); serialEvent(); if (stringComplete) { //Serial.println(inputString); gPC_T = atoi(inputString); gi=0; stringComplete= false; } if(gCT > 400) {gCT=0;WiFi_loop();} else{gCT++;} } void WiFi_loop() { WiFiClient client; if (!client.connect(host, httpPort)) {//TCPサーバへの接続要求 } else { if(gSendText.length() > 1) { client.print(gSendText);//データを送信 gSendText=""; } else{client.print("a");}//"a"を送信 delay(10); // Read all the lines of the reply from server and print them to Serial while(client.available()) { String line = client.readStringUntil('\n');//受信します。 if(line.indexOf("BT") == 0) { int Bracket2 = line.indexOf('#', 2 ); int Bracket3 = line.indexOf('#',Bracket2+1); //Serial.println(Bracket2); //Serial.println(Bracket3); String inString =line.substring(Bracket2+1,Bracket3); gPC_T = inString.toInt(); //Serial.print(inString+" ******\r\n"); } Serial.print(line+"\r\n"); gSendText=line + "=OK\r\n";//送信データのセット } } } void DC_Servo() { String str; int PS1=digitalRead(14); int PS2=digitalRead(16); long T1,T2; long DPC=gPC_T-gPC_N; if( DPC > 0) { T1=DPC*400; if(T1 > 1000){T1=1000;} T2=1000-T1; digitalWrite(12,LOW); digitalWrite(13,HIGH); delayMicroseconds(T1); digitalWrite(12, HIGH); digitalWrite(13, HIGH); delayMicroseconds(T2); } else { T1=(1-DPC)*400; if(T1 > 1000){T1=1000;} T2=1000-T1; digitalWrite(12, HIGH); digitalWrite(13, LOW); delayMicroseconds(T1); digitalWrite(12, HIGH); digitalWrite(13, HIGH); delayMicroseconds(T2); } str=String(gPC_T)+" "+String(gPC_N); if(PS1B==HIGH) { if(PS1==LOW) { PS1B=LOW; if(PS2==HIGH) { gPC_N++; Serial.println(str); } else { gPC_N--; Serial.println(str); } } } else { if(PS1==HIGH) { PS1B=HIGH; if(PS2==LOW) { gPC_N++; Serial.println(str); } else { gPC_N--; Serial.println(str); } } } } void serialEvent() { while (Serial.available()) { char inChar = (char)Serial.read(); inputString[gi]=inChar; if (inChar == '\n') { stringComplete = true; inputString[gi-1]=0; } gi++; } }
//TCP_Server_ADXL345_HMC5883_3 // 30章モータPWN10EB12CBのサーボ制御 import processing.net.*; int port = 13000; boolean myServerRunning = true; Server myServer; import processing.serial.*; Serial myPort; // The serial port float Mx=0; float My=0; float Mz=0; int gC=0;int gF=0;int gN=20; int[] Ax=new int[gN+1]; int[] Ay=new int[gN+1]; int[] Az=new int[gN+1]; float Dx=0,Dy=0,Dz=0; int gC0=0,gCmax=4; float Dxnew=0,Dynew=0,Dznew=0,Dxold,Dyold,Dzold; float Mxnew=0,Mynew=0,Mznew=0,Mxold,Myold,Mzold; int gM=0; void setup () { size(740, 660, P3D); myServer = new Server(this, port); // Starts a myServer on port 13000 } void draw () { int i; int Sx=0,Sy=0,Sz=0; background(0, 256, 256); Client thisClient = myServer.available(); if (thisClient != null) { if (thisClient.available() > 0) { String myString =thisClient.readString(); //myString = trim(myString); int sensors[] = int(split(myString, ',')); int sensorNum; for (sensorNum = 0; sensorNum < sensors.length; sensorNum++) { print(sensors[sensorNum] + "\t"); } println(); int f; if(sensorNum==6){f=1;} else { f=0; int MX=(int)(100*Mx); thisClient.write("BT"+gM+"#"+MX+"#\r\n"); } if(f==1) { Mxold=Mxnew;Mxnew=atan2(sensors[0],sensors[2]); Myold=Mynew;Mynew=atan2(sensors[1],sensors[2]); Mzold=Mznew;Mznew=atan2(sensors[4],sensors[3]); if(abs(Mxnew)>abs(Mynew)) { if(abs(Mxnew)<0.2) {gM=0;} else { if(abs(Mxnew)<0.4) {gM=3;} else{gM=1;} } } else { if(abs(Mynew)<0.2) {gM=0;} else { if(abs(Mynew)<0.4) {gM=4;} else{gM=2;} } } Ax[gC]=sensors[0]; Ay[gC]=sensors[1]; Az[gC]=sensors[2]; if(gF==1) { for(i=0;i<gN;i++) { Sx=Sx+Ax[i]; Sy=Sy+Ay[i]; Sz=Sz+Az[i]; } Dxold=Dxnew;Dxnew=0.1*(Ax[gC]-Sx/gN); Dyold=Dynew;Dynew=0.1*(Ay[gC]-Sy/gN); Dzold=Dznew;Dznew=0.1*(Az[gC]-Sz/gN); } gC++; if(gC > gN) {gF=1;gC=0;} gCmax=gC0; gC0=0; } } } if(gF==1) { Mx=Mxold+(Mxnew-Mxold)*gC0/gCmax; My=Myold+(Mynew-Myold)*gC0/gCmax; Mz=Mzold+(Mznew-Mzold)*gC0/gCmax; Dx=Dxold+(Dxnew-Dxold)*gC0/gCmax; Dy=Dyold+(Dynew-Dyold)*gC0/gCmax; Dz=Dzold+(Dznew-Dzold)*gC0/gCmax; println(Mx+" " +My +" " +Mz+" "+Dx+" " +Dy +" " +Dz); } gC0++; lights(); translate(width / 2, height / 2,-200); rotateX(Mx); rotateY(Mz); rotateZ(My); noStroke(); fill(240, 240, 230); translate(0,0,100); translate(Dx,Dz,Dy); OB_A2();//Z軸回転体(胴体) OB_A3();//Z軸回転体(胴体先端) OB2();//主翼 OB3();//水平尾翼 OB4();//垂直尾翼 OB_A4();//Z軸回転体(エンジン) } void OB_A4()//Z軸回転体(エンジン) { int sides=16;//分割数 int Sn=6;//面数 float Pz[]={0,-1,-50,-60,-70,-70};//z座標 float R[]={15,20,20,15,15,1};//回転物半径 int i,j; float angleIncrement = TWO_PI/sides; pushMatrix(); translate(60, 36.3,-145); for(j=0;j<Sn-1;j++) { float angle = 0; beginShape(QUAD_STRIP); for (i = 0; i < sides + 1; ++i) { vertex(R[j]*cos(angle), R[j]*sin(angle), Pz[j]); vertex(R[j+1]*cos(angle), R[j+1]*sin(angle), Pz[j+1]); angle += angleIncrement; } endShape(CLOSE); } popMatrix(); pushMatrix(); translate(-60, 36.3,-145); for(j=0;j<Sn-1;j++) { float angle = 0; beginShape(QUAD_STRIP); for (i = 0; i < sides + 1; ++i) { vertex(R[j]*cos(angle), R[j]*sin(angle), Pz[j]); vertex(R[j+1]*cos(angle), R[j+1]*sin(angle), Pz[j+1]); angle += angleIncrement; } endShape(CLOSE); } popMatrix(); } void OB4()//垂直尾翼 { float Px[]={0,0,3,-3,0,0,2,-2};//ポイントx float Py[]={0,0,0,0,-80,-80,-80,-80};//ポイントy float Pz[]={25,-25,0,0,0,-25,-15,-15};//ポイントz int Sn=10;//面数 int S1[]={0,0,1,1,0,0,1,1,4,5};//面ポイント1 int S2[]={2,4,2,5,3,4,3,5,6,6};//面ポイント2 int S3[]={6,6,5,6,7,7,7,7,7,7};//面ポイント3 int i; pushMatrix(); translate(0, 0,-395); for(i=0;i<Sn;i++) { beginShape(); vertex(Px[S1[i]], Py[S1[i]],Pz[S1[i]]); vertex(Px[S2[i]], Py[S2[i]],Pz[S2[i]]); vertex(Px[S3[i]], Py[S3[i]],Pz[S3[i]]); endShape(CLOSE); } popMatrix(); } void OB3()//水平尾翼 { float Px[]={0,0,0,100,100,100};//ポイントx float Py[]={3,3,-3,2,2,-2};//ポイントy float Pz[]={15,-15,0,-5,-15,-10};//ポイントz int Sn=7;//面数 int S1[]={0,0,0,0,1,1,3};//面ポイント1 int S2[]={1,3,2,3,2,4,4};//面ポイント2 int S3[]={4,4,5,5,5,5,5};//面ポイント3 int i; pushMatrix(); translate(0, 0,-415); for(i=0;i<Sn;i++) { beginShape(); vertex(Px[S1[i]], Py[S1[i]],Pz[S1[i]]); vertex(Px[S2[i]], Py[S2[i]],Pz[S2[i]]); vertex(Px[S3[i]], Py[S3[i]],Pz[S3[i]]); endShape(CLOSE); } for(i=0;i<Sn;i++) { beginShape(); vertex(-Px[S1[i]], Py[S1[i]],Pz[S1[i]]); vertex(-Px[S2[i]], Py[S2[i]],Pz[S2[i]]); vertex(-Px[S3[i]], Py[S3[i]],Pz[S3[i]]); endShape(CLOSE); } popMatrix(); } void OB2()//主翼 { float Px[]={10,10,10,220,220,220};//ポイントx float Py[]={20,20,10,15,15,10};//ポイントy float Pz[]={40,-40,0,10,-10,0};//ポイントz int Sn=7;//面数 int S1[]={0,0,0,0,1,1,3};//面ポイント1 int S2[]={1,3,2,3,2,4,4};//面ポイント2 int S3[]={4,4,5,5,5,5,5};//面ポイント3 int i; pushMatrix(); translate(0, 0,-200); for(i=0;i<Sn;i++) { beginShape(); vertex(Px[S1[i]], Py[S1[i]],Pz[S1[i]]); vertex(Px[S2[i]], Py[S2[i]],Pz[S2[i]]); vertex(Px[S3[i]], Py[S3[i]],Pz[S3[i]]); endShape(CLOSE); } for(i=0;i<Sn;i++) { beginShape(); vertex(-Px[S1[i]], Py[S1[i]],Pz[S1[i]]); vertex(-Px[S2[i]], Py[S2[i]],Pz[S2[i]]); vertex(-Px[S3[i]], Py[S3[i]],Pz[S3[i]]); endShape(CLOSE); } popMatrix(); } void OB_A3()//Z軸回転体(胴体先端) { int sides=16;//分割数 int Sn=8;//面数 float Pz[]={0,-3.2,-11.7,-21.8,-31.9,-45.3,-70.2,-129.4};//z座標 float R[]={0.1,3.7,7.4,9.6,11.8,13.8,17.4,17.7};//回転物半径 int i,j; float angleIncrement = TWO_PI/sides; pushMatrix(); translate(0, 8.5); for(j=0;j<Sn-1;j++) { float angle = 0; beginShape(QUAD_STRIP); for (i = 0; i < sides + 1; ++i) { vertex(R[j]*cos(angle), R[j]*sin(angle), Pz[j]); vertex(R[j+1]*cos(angle), R[j+1]*sin(angle), Pz[j+1]); angle += angleIncrement; } endShape(CLOSE); } popMatrix(); } void OB_A2()//Z軸回転体(胴体) { int sides=16;//分割数 int Sn=13;//面数 float Pz[]={-40,-46.9,-68.4,-88.2,-108.8,-129.4,-319.4,-346.2,-373,-391.2,-409.4,-433,-440};//z座標 float R[]={0.1,7.8,15.4,19.5,22.3,25,25,22.3,18.7,16,13.3,8.3,0.1};//回転物半径 int i,j; float angleIncrement = TWO_PI/sides; for(j=0;j<Sn-1;j++) { float angle = 0; beginShape(QUAD_STRIP); for (i = 0; i < sides + 1; ++i) { fill(240, 240, 230); if(j==1){fill(20, 20, 40);} if(j==5 && i==0){fill(20, 20, 40);} if(j==5 && i==8){fill(20, 20, 40);} if(j==5 && i==16){fill(20, 20, 40);} vertex(R[j]*cos(angle), R[j]*sin(angle), Pz[j]); vertex(R[j+1]*cos(angle), R[j+1]*sin(angle), Pz[j+1]); angle += angleIncrement; } endShape(CLOSE); } }
//ADDXL345_HMC5883 27章WiFi無線でprocessingの3Dサンプルを動かす #include <Wire.h> #include <Adafruit_Sensor.h> #include <Adafruit_ADXL345_U.h> #include <stdio.h> //WiFiClient 非同期 #include <ESP8266WiFi.h> const char* ssid = "SSID";//無線LANのSSIDを設定します。 const char* password = "password";//無線LANのpasswordを設定します。 const char* host = "192.168.11.2";//パソコンのIPアドレスを設定します。 const int httpPort = 13000;//TCPサーバのポート static String gSendText=""; /* Assign a unique ID to this sensor at the same time */ Adafruit_ADXL345_Unified accel = Adafruit_ADXL345_Unified(12345); #include <Adafruit_HMC5883_U.h> Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345); void displaySensorDetails(void) { sensor_t sensor; accel.getSensor(&sensor); Serial.println("------------------------------------"); Serial.print ("Sensor: "); Serial.println(sensor.name); Serial.print ("Driver Ver: "); Serial.println(sensor.version); Serial.print ("Unique ID: "); Serial.println(sensor.sensor_id); Serial.print ("Max Value: "); Serial.print(sensor.max_value); Serial.println(" m/s^2"); Serial.print ("Min Value: "); Serial.print(sensor.min_value); Serial.println(" m/s^2"); Serial.print ("Resolution: "); Serial.print(sensor.resolution); Serial.println(" m/s^2"); Serial.println("------------------------------------"); Serial.println(""); delay(500); } void displayDataRate(void) { Serial.print ("Data Rate: "); switch(accel.getDataRate()) { case ADXL345_DATARATE_3200_HZ: Serial.print ("3200 "); break; case ADXL345_DATARATE_1600_HZ: Serial.print ("1600 "); break; case ADXL345_DATARATE_800_HZ: Serial.print ("800 "); break; case ADXL345_DATARATE_400_HZ: Serial.print ("400 "); break; case ADXL345_DATARATE_200_HZ: Serial.print ("200 "); break; case ADXL345_DATARATE_100_HZ: Serial.print ("100 "); break; case ADXL345_DATARATE_50_HZ: Serial.print ("50 "); break; case ADXL345_DATARATE_25_HZ: Serial.print ("25 "); break; case ADXL345_DATARATE_12_5_HZ: Serial.print ("12.5 "); break; case ADXL345_DATARATE_6_25HZ: Serial.print ("6.25 "); break; case ADXL345_DATARATE_3_13_HZ: Serial.print ("3.13 "); break; case ADXL345_DATARATE_1_56_HZ: Serial.print ("1.56 "); break; case ADXL345_DATARATE_0_78_HZ: Serial.print ("0.78 "); break; case ADXL345_DATARATE_0_39_HZ: Serial.print ("0.39 "); break; case ADXL345_DATARATE_0_20_HZ: Serial.print ("0.20 "); break; case ADXL345_DATARATE_0_10_HZ: Serial.print ("0.10 "); break; default: Serial.print ("???? "); break; } Serial.println(" Hz"); } void displayRange(void) { Serial.print ("Range: +/- "); switch(accel.getRange()) { case ADXL345_RANGE_16_G: Serial.print ("16 "); break; case ADXL345_RANGE_8_G: Serial.print ("8 "); break; case ADXL345_RANGE_4_G: Serial.print ("4 "); break; case ADXL345_RANGE_2_G: Serial.print ("2 "); break; default: Serial.print ("?? "); break; } Serial.println(" g"); } void setup(void) { Serial.begin(115200); Serial.println("Accelerometer Test"); Serial.println(""); /* Initialise the sensor */ if(!accel.begin()) { /* There was a problem detecting the ADXL345 ... check your connections */ Serial.println("Ooops, no ADXL345 detected ... Check your wiring!"); while(1); } /* Set the range to whatever is appropriate for your project */ accel.setRange(ADXL345_RANGE_16_G); //displaySetRange(ADXL345_RANGE_8_G); //displaySetRange(ADXL345_RANGE_4_G); //displaySetRange(ADXL345_RANGE_2_G); /* Display some basic information on this sensor */ displaySensorDetails(); /* Display additional settings (outside the scope of sensor_t) */ displayDataRate(); displayRange(); Serial.println(""); if(!mag.begin()) { //Serial.println("Ooops, no HMC5883 detected ... Check your wiring!"); while(1); } WiFi.begin(ssid, password);//無線LANに接続要求 while (WiFi.status() != WL_CONNECTED) {//接続完了まで待ちます。 delay(500); Serial.print("."); } Serial.println(""); Serial.println("WiFi connected"); } void loop(void) { /* Get a new sensor event */ sensors_event_t event; accel.getEvent(&event); sensors_event_t event2; mag.getEvent(&event2); int Xd=(int)(1000*event.acceleration.x); int Yd=(int)(1000*event.acceleration.y); int Zd=(int)(1000*event.acceleration.z); int Mx=(int)(100*event2.magnetic.x); int My=(int)(100*event2.magnetic.y); int Mz=(int)(100*event2.magnetic.z); char str[256]; sprintf(str,"%d,%d,%d,%d,%d,%d\r\n",Xd,Yd,Zd,Mx,My,Mz); //Serial.print(str); /* Display the results (acceleration is measured in m/s^2) */ //Serial.print("X: "); Serial.print(Xd); Serial.print(","); //Serial.print("Y: "); Serial.print(Yd); Serial.print(","); //Serial.print("Z: "); Serial.print(Zd); Serial.print(","); //Serial.print("Mx: "); Serial.print(Mx); Serial.print(","); //Serial.print("My: "); Serial.print(My); Serial.print(","); //Serial.print("Mz: "); Serial.print(Mz); Serial.print(","); delay(200); // Use WiFiClient class to create TCP connections WiFiClient client; if (client.connect(host, httpPort)) { client.print(str); } }