コウスケ スズキ
lpc1768 aassssq
Dependencies: mbed Servo LPS25HB_I2C MPU6050 SDFileSystem
Diff: main.cpp
- Revision:
- 3:14b178724982
- Parent:
- 2:194e00108f75
- Child:
- 4:ec40fbfb90f6
diff -r 194e00108f75 -r 14b178724982 main.cpp
--- a/main.cpp Thu Feb 17 15:08:56 2022 +0000
+++ b/main.cpp Fri Feb 25 06:06:57 2022 +0000
@@ -1,7 +1,19 @@
+//｢変更｣と書かれている場所の数値は、自分で決めて変える。（主にセンサー数値の補正）
#include "mbed.h"
#include "MPU6050.h"
#include "LPS.h"
#include "SDFileSystem.h"
+#include "Servo.h"
+
+const double dt = 0.01; //微小時間(変更)
+
+const double Kp =5; //変更
+const double Ki =3; //変更
+const double Kd =3; //変更
+
+const double mc = 1995; //最大トルク(変更)
+
+const double T =0; //目標角度°(変更)
I2C i2c(p28,p27);
LPS ps(i2c);
@@ -10,114 +22,208 @@
Serial xbee(p13,p14);
Serial pc(USBTX,USBRX);
+Servo motor1(p21);
+Servo motor2(p22);
+Servo motor3(p23);
+Servo motor4(p24);
+
+Timer ti;
+
+int cmd = xbee.getc();
+
int gyro[3];
int accel[3];
+int count;
double GX,GY,GZ;
double Tgx,Tgy,Tgz;
+
+
int main() {
- xbee.printf("start\r\n");
- pc.printf("start\r\n");
+ //モーター準備開始(一時待つ事が必要)
+ motor1 = motor2 = motor3 = motor4 = 0.0;
+ wait(0.5);
+ motor1 = motor2 = motor3 = motor4 = 1.0;
+ wait(8);
+ motor1 = motor2 = motor3 = motor4 = 0.0;
+ wait(8);
+ //モーター準備終了
- GX = 0,GY = 0,GZ = 0;
- Tgx = 0,Tgy = 0,Tgz = 0;
+ //pcのxbeeからのコマンド｢c｣で開始
+ if(cmd = 'c'){
- if (!ps.init()){
- pc.printf("Failed to autodetect pressure sensor!\r\n");
- while (1);
- }
+ xbee.printf("start\r\n");
+ pc.printf("start\r\n");
- ps.enableDefault();
+ count = 0;
+ GX = 0,GY = 0,GZ = 0;
+ Tgx = 0,Tgy = 0,Tgz = 0;
- mkdir("/sd/test1", 0777);
- FILE *fp = fopen("/sd/test1/sdtest1.txt", "w");
+ if (!ps.init()){
+ pc.printf("Failed to autodetect pressure sensor!\r\n");
+ while (1);
+ }
+
+ ps.enableDefault();
- fprintf(fp,"start\r\n");
+ mkdir("/sd/test1", 0777);
+ FILE *fp = fopen("/sd/test1/sdtest1.txt", "w");
- for(int k = 0; k < 50; k++){
+ fprintf(fp,"start\r\n");
+
+ ti.start();
+ while(1){
- if(fp == NULL) {
- error("Could not open file for write\n");
- }
+ if(fp == NULL) {
+ error("Could not open file for write\n");
+ }
- mpu.readGyroData(gyro);
- int gx = gyro[0]+146;
- int gy = gyro[1]+232; //まだ
- int gz = gyro[2]+7;
- //printf("%d %d %d\r\n",gx,gy,gz);
+
+ //角度算出
+ mpu.readGyroData(gyro);
+ int gx = gyro[0]+3656-3505-50-1850+2223-24; //変更
+ int gy = gyro[1]-30+710-140-300+375-50; //変更
+ int gz = gyro[2]+5+195-453+300-244-20-10; //変更
+ //printf("%d %d %d\r\n",gx,gy,gz);
- double gX = gx*0.02562; //0.0128114995(測定レンジ±500)
- double gY = gy*0.02562; //0.02562299(±250)
- double gZ = gz*0.02562;
+ double gX = gx*0.02562; //0.0128114995(測定レンジ±500) //変更
+ double gY = gy*0.02562; //0.02562299(±250) //変更
+ double gZ = gz*0.02562; //変更
- int gX1 = gX;
- int gY1 = gY;
- int gZ1 = gZ;
+ int gX1 = gX;
+ int gY1 = gY;
+ int gZ1 = gZ;
- double gX2 = gX1*0.01;
- double gY2 = gY1*0.01;
- double gZ2 = gZ1*0.01;
+ double gX2 = gX1*dt;
+ double gY2 = gY1*dt;
+ double gZ2 = gZ1*dt;
- GX = GX + gX2;
- GY = GY + gY2;
- GZ = GZ + gZ2;
+ GX = GX + gX2;
+ GY = GY + gY2;
+ GZ = GZ + gZ2;
- Tgx = Tgx + abs(gX2);
- Tgy = Tgy + abs(gY2);
- Tgz = Tgz + abs(gZ2);
+ Tgx = Tgx + abs(gX2);
+ Tgy = Tgy + abs(gY2);
+ Tgz = Tgz + abs(gZ2);
- if(Tgx > 10){
- if(GX > 0){
- GX = GX - 0.3;//変更必須
- }else{
- GX = GX + 0.3;
+ //和の補正(精度はよくない)
+ if(Tgx > 3){
+ if(GX > 0){
+ GX = GX - 0.3;//変更
+ }else{
+ GX = GX + 0.3;//変更
+ }
+ Tgx = 0;
}
- Tgx = 0;
- }
- if(Tgy > 10){
- if(GY >0){
- GY = GY - 0.3;
- }else{
- GY = GY + 0.3;
- }
- Tgy = 0;
- }
- if(Tgz > 10){
- if(GZ >0){
- GZ = GZ - 0.3;
- }else{
- GZ = GZ + 0.3;
- }
- Tgz = 0;
- }
+ if(Tgy > 3){
+ if(GY >0){
+ GY = GY - 0.3;//変更
+ }else{
+ GY = GY + 0.3;//変更
+ }
+ Tgy = 0;
+ }
+ if(Tgz > 3){
+ if(GZ >0){
+ GZ = GZ - 0.3;//変更
+ }else{
+ GZ = GZ + 0.3;//変更
+ }
+ Tgz = 0;
+ }
- mpu.readAccelData(accel);
- int ax = accel[0]-123;//x軸方向の加速度
- int ay = accel[1]+60;//y軸方向の加速度
- int az = accel[2]+1110 ;//z軸方向の加速度
- float AX = ax*0.000597964111328125;
- float AY = ay*0.000597964111328125;
- float AZ = az*0.000597964111328125;
- double a = AX*AX+AY*AY+AZ*AZ-95.982071137936;
+
+ //加速度算出
+ mpu.readAccelData(accel);
+ int ax = accel[0]-123; //変更
+ int ay = accel[1]+60; //変更
+ int az = accel[2]+1110 ; //変更
+ float AX = ax*0.000597964111328125; //変更
+ float AY = ay*0.000597964111328125; //変更
+ float AZ = az*0.000597964111328125; //変更
+ double a = AX*AX+AY*AY+AZ*AZ-95.982071137936;
+
+
+ //気圧・標高・温度算出
+ float pres = ps.readPressureMillibars();
+ float altit = ps.pressureToAltitudeMeters(pres);
+ float tempe = ps.readTemperatureC();
+
- float pres = ps.readPressureMillibars();
- float altit = ps.pressureToAltitudeMeters(pres);
- float tempe = ps.readTemperatureC();
+ //PID開始
+ //必要なトルクを算出
+ double c1,c2;
+ double f1,f2;
+ f1 = T - GX;
+ f2 = T - GY;
+
+ c1 = Kp*f1+Ki*f1*dt+Kd*gX1; //トルク
+ c2 = Kp*f2+Ki*f2*dt+Kd*gY1;
+ //printf("%.2f %.2f\r\n",c1,c2); //トルク
+
+ //回転トルク→duty比を算出
+ double d1,d2;
+ d1 = c1/mc;
+ d2 = c2/mc;
+
+ if(d1 > 0.9){ //変更
+ d1 = 0.9; //変更
+ }
+ if(d2 > 0.9){ //変更
+ d2 = 0.9; //変更
+ }
+ //printf("%.2f %.2f\r\n",d1,d2); //duty比
+
+
+ //試行回数
+ count = count + 1;
- pc.printf("%.2f %.2f %.2f %.2f\r\n",AX,AY,AZ,a);
- pc.printf("%.2f %.2f %.2f\r\n",GX,GY,GZ);
- pc.printf("%.2f %.2f %.2f\r\n",pres,altit,tempe);
+ //duty比からmotorを動かす。
+ if(GX > 0){
+ motor1 = abs(d1);
+ motor2 = 0.5-abs(d1)/2; //変更
+ }else if(c1 < 0){
+ motor2 = abs(d1);
+ motor1 = 0.5-abs(d1)/2; //変更
+ }else{
+ motor1 = motor2 = 0;
+ }
+
+ if(GX > 0){
+ motor3 = abs(d2);
+ motor4 = 0.5-abs(d2)/2; //変更
+ }else if(c2 < 0){
+ motor4 = abs(d2);
+ motor3 = 0.5-abs(d2)/2; //変更
+ }else{
+ motor3 = motor4 = 0;
+ }
- xbee.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",AX,AY,AZ,a,GX,GY,GZ,pres,altit,tempe);
- fprintf(fp, "%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",AX,AY,AZ,a,GX,GY,GZ,pres,altit,tempe);
- wait(0.01);
+ /*主なセンサー表示
+ pc.printf("T %.2f\r\n",ti.read());
+ pc.printf("c %d\r\n",count);
+ pc.printf("ax%.2f ay%.2f az%.2f |a|%.2f\r\n",AX,AY,AZ,a);
+ pc.printf("gx%.2f gy%.2f gz%.2f\r\n",GX,GY,GZ);
+ pc.printf("p%.2f alt%.2f t%.2f\r\n",pres,altit,tempe);
+ */
+
+ pc.printf("c%d, T%.2f, ax%.2f,ay%.2f,az%.2f,|a|%.2f, gx%.2f,gy%.2f,gz%.2f, p%.2f,al%.2f,t%.2f\r\n",count,ti.read(),AX,AY,AZ,a,GX,GY,GZ,pres,altit,tempe);
+ xbee.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",AX,AY,AZ,a,GX,GY,GZ,pres,altit,tempe);
+ fprintf(fp, "%d %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f\r\n",count,ti.read(),AX,AY,AZ,a,GX,GY,GZ,pres,altit,tempe,c1,c2,d1,d2);
+ //fprintf(fp,"%.2f %.2f %.2f %.2f %.2f\r\n",ti.read(),c1,c2,d1,d2); //調整用
+
+ wait(dt);
+
+ if(AZ > 9 && AZ < 11 && a > 8 && a < 11 && 3 < ti.read()){
+ fprintf(fp,"end\r\n");
+ fclose(fp); //注意：ここまで行わないと、SDカードに保存されない
+ xbee.printf("end\r\n");
+ pc.printf("end\r\n");
+ ti.stop();
+ return 0;
+ }
+
+ }
}
-
- fprintf(fp,"end\r\n");
- fclose(fp);
- xbee.printf("end\r\n");
- pc.printf("end\r\n");
-
- return 0;
}
\ No newline at end of file