NagaokaRoboticsClub_mbedTeam
2022NHKAチーム(射出、紙飛行機折り、昇降)
Dependencies: OmniPosition PID R1370 SerialMultiByte Servo ikarashiMDC_2byte_ver mbed omni_wheel
Diff: main.cpp
- Revision:
- 2:f856cbeb5940
- Parent:
- 1:31f47d3fa8cd
--- a/main.cpp Mon Oct 10 05:43:42 2022 +0000
+++ b/main.cpp Mon Oct 10 09:12:26 2022 +0000
@@ -46,22 +46,22 @@
Servo pwm_imput2(BLDC2);
Servo pwm_imput3(BLDC3);
-//abe 足回り
-double engine[4];
-double spin_power;
-double posiX , posiY , posiTheta;
-int TargetAngle = 0;
-int StartAngle = 0;
+////abe 足回り
+//double engine[4];
+//double spin_power;
+//double posiX , posiY , posiTheta;
+//int TargetAngle = 0;
+//int StartAngle = 0;
-OmniWheel omni(4);
-OmniPosition posi(sub1TX, sub1RX);
-
-PID angle(10.0, 5.0, 0.0000005, 0.01);
+//OmniWheel omni(4);
+//OmniPosition posi(sub1TX, sub1RX);
+//
+//PID angle(10.0, 5.0, 0.0000005, 0.01);
-//プロトタイプ宣言
-void chassis(); //足回りの制御・ジャイロ・テラターム
-void spin(double ang); //PID
-int pm(double num); //正負判定
+////プロトタイプ宣言
+//void chassis(); //足回りの制御・ジャイロ・テラターム
+//void spin(double ang); //PID
+//int pm(double num); //正負判定
Timer t;
@@ -88,17 +88,18 @@
mycon.StartReceive(); //コントローラー受信・宣言
- //足回り宣言
- omni.wheel[0].setRadian(PI * 1.0 / 4.0);
- omni.wheel[1].setRadian(PI * 3.0 / 4.0);
- omni.wheel[2].setRadian(PI * 5.0 / 4.0);
- omni.wheel[3].setRadian(PI * 7.0 / 4.0);
-
- angle.setInputLimits(-180,180);
- angle.setOutputLimits(-0.4,0.4);
- angle.setBias(0);
- angle.setMode(1);
- angle.setSetPoint(0);
+
+// //足回り宣言
+// omni.wheel[0].setRadian(PI * 1.0 / 4.0);
+// omni.wheel[1].setRadian(PI * 3.0 / 4.0);
+// omni.wheel[2].setRadian(PI * 5.0 / 4.0);
+// omni.wheel[3].setRadian(PI * 7.0 / 4.0);
+//
+// angle.setInputLimits(-180,180);
+// angle.setOutputLimits(-0.4,0.4);
+// angle.setBias(0);
+// angle.setMode(1);
+// angle.setSetPoint(0);
int16_t angle[4] = {0};//エンコーダ受信格納
@@ -153,8 +154,8 @@
}
- /*阿部君の素晴らしい天才的な足回り関数*/
- chassis();
+// /*阿部君の素晴らしい天才的な足回り関数*/
+// chassis();
rcv.getData(pulse); //エンコーダ受信
for(int i=0,j=0;i<4;i++,j+=2){
@@ -213,13 +214,13 @@
}
//機構昇降
if(b[9]){
- speed[8] = -0.35;
+ speed[8] = -0.32;
}else
if(b[13]){
speed[8] = 0.4;
}
if(b[11]){
- speed[9] = -0.35;
+ speed[9] = -0.32;
}else
if(b[14]){
speed[9] = 0.4;
@@ -236,72 +237,72 @@
}
-void chassis(){
-
- mycon.getData(data);
- for (int i=0; i<4; i++) {
- stick[i] = data[i+2];
- }
-
- /*ジャイロ読み取り*/
- posiX = posi.getX();
- posiY = posi.getY();
- posiTheta = posi.getTheta()*(180.0/M_PI);//OmniPositionライブラリが弧度法で角度をつけていたため60分法に変換
- pc.printf("x:%5.2f Y:%5.2f theta:%0.3f | ",posiX, posiY,posiTheta);
-
- if(abs(stick[2]) < 10){
- if(fabs(TargetAngle-posiTheta)>8){
- TargetAngle += 15*pm(posiTheta-TargetAngle);
- if(abs(TargetAngle)==195){//abs(TargetAngle)==180だと永遠にこのif文に捕らわれてしまいそう。
- TargetAngle += -360*pm(TargetAngle);
- }
- }
- spin(TargetAngle);
- }
-
- /*全方位*/
- for(int i=0; i<4; i++){
- if(abs(stick[i]) > 10){
- if(trigger[1]<10) trigger[1] = 0;
- engine[i] = maxSpeed*(stick[i]/128.0)*(trigger[1]/255.0);
- }else if(b[i]%2){
- if(trigger[1]<10) trigger[1] = 0;
- //b[1]のとき左向き（負）b[3]のとき右向き（正）
- engine[0] = maxSpeed*pm(i-2)*(trigger[1]/255.0);
- engine[1] = 0;
- }else if(b[i]%2==0){
- if(trigger[1]<10) trigger[1] = 0;
- //b[0]のとき上向き（正）b[2]のとき下向き（負）
- engine[1] = -maxSpeed*pm(i-1)*(trigger[1]/255.0);
- engine[0] = 0;
- }else{
- engine[i] = 0;
- }
- }
- /*旋回*/
- if(abs(stick[2]) > 10){
- spin_power = engine[2];
- }else{
- spin_power = angle.compute();
- }
-
- omni.computeXY(engine[0],engine[1],-spin_power);
- for(int i = 0; i < 4; i++) speed[i] = omni.wheel[i];
- for(int i=0; i<4; i++) motor[i].setSpeed(speed[i]);
-
-}
-
-
-void spin(double ang)
-{
- angle.setSetPoint(ang);
- posiTheta = posi.getTheta()*(180.0/M_PI);
- angle.setProcessValue(posiTheta);
- pc.printf("ang:%.2f pid:%.2f posi:%.2f T-p:%.2f\r\n",ang,-angle.compute(),posiTheta,TargetAngle-posiTheta);
- //for(int i=4; i<12; i++) motor[i].setSpeed(0);射出機構とかのモーターが出てきたときに[//]を消す
-}
-
-
-int pm(double num){
- return((num>0)-(num<0));
-}
\ No newline at end of file
+//void chassis(){
+//
+// mycon.getData(data);
+// for (int i=0; i<4; i++) {
+// stick[i] = data[i+2];
+// }
+//
+// /*ジャイロ読み取り*/
+// posiX = posi.getX();
+// posiY = posi.getY();
+// posiTheta = posi.getTheta()*(180.0/M_PI);//OmniPositionライブラリが弧度法で角度をつけていたため60分法に変換
+// pc.printf("x:%5.2f Y:%5.2f theta:%0.3f | ",posiX, posiY,posiTheta);
+//
+// if(abs(stick[2]) < 10){
+// if(fabs(TargetAngle-posiTheta)>8){
+// TargetAngle += 15*pm(posiTheta-TargetAngle);
+// if(abs(TargetAngle)==195){//abs(TargetAngle)==180だと永遠にこのif文に捕らわれてしまいそう。
+// TargetAngle += -360*pm(TargetAngle);
+// }
+// }
+// spin(TargetAngle);
+// }
+//
+// /*全方位*/
+// for(int i=0; i<4; i++){
+// if(abs(stick[i]) > 10){
+// if(trigger[1]<10) trigger[1] = 0;
+// engine[i] = maxSpeed*(stick[i]/128.0)*(trigger[1]/255.0);
+// }else if(b[i]%2){
+// if(trigger[1]<10) trigger[1] = 0;
+// //b[1]のとき左向き（負）b[3]のとき右向き（正）
+// engine[0] = maxSpeed*pm(i-2)*(trigger[1]/255.0);
+// engine[1] = 0;
+// }else if(b[i]%2==0){
+// if(trigger[1]<10) trigger[1] = 0;
+// //b[0]のとき上向き（正）b[2]のとき下向き（負）
+// engine[1] = -maxSpeed*pm(i-1)*(trigger[1]/255.0);
+// engine[0] = 0;
+// }else{
+// engine[i] = 0;
+// }
+// }
+// /*旋回*/
+// if(abs(stick[2]) > 10){
+// spin_power = engine[2];
+// }else{
+// spin_power = angle.compute();
+// }
+//
+// omni.computeXY(engine[0],engine[1],-spin_power);
+// for(int i = 0; i < 4; i++) speed[i] = omni.wheel[i];
+// for(int i=0; i<4; i++) motor[i].setSpeed(speed[i]);
+//
+//}
+//
+//
+//void spin(double ang)
+//{
+// angle.setSetPoint(ang);
+// posiTheta = posi.getTheta()*(180.0/M_PI);
+// angle.setProcessValue(posiTheta);
+// pc.printf("ang:%.2f pid:%.2f posi:%.2f T-p:%.2f\r\n",ang,-angle.compute(),posiTheta,TargetAngle-posiTheta);
+// //for(int i=4; i<12; i++) motor[i].setSpeed(0);射出機構とかのモーターが出てきたときに[//]を消す
+//}
+//
+//
+//int pm(double num){
+// return((num>0)-(num<0));
+//}
\ No newline at end of file