abe takumi
q
Dependencies: FEP_RX22 OmniPosition PID R1370 ikarashiMDC_2byte_ver omni_wheel
Revision 0:b7dbdc0b19f3, committed 22 months ago
- Comitter:
- me33004m
- Date:
- Mon Oct 10 08:01:10 2022 +0000
- Commit message:
- compureat;
Changed in this revision
diff -r 000000000000 -r b7dbdc0b19f3 FEP_RX22.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FEP_RX22.lib Mon Oct 10 08:01:10 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/FEP_RX22/#39eb865e21e1
diff -r 000000000000 -r b7dbdc0b19f3 OmniPosition.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/OmniPosition.lib Mon Oct 10 08:01:10 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/piroro4560/code/OmniPosition/#893d1d146757
diff -r 000000000000 -r b7dbdc0b19f3 PID.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Mon Oct 10 08:01:10 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/aberk/code/PID/#6e12a3e5af19
diff -r 000000000000 -r b7dbdc0b19f3 R1370.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/R1370.lib Mon Oct 10 08:01:10 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/R1370/#243364135087
diff -r 000000000000 -r b7dbdc0b19f3 ikarashiMDC_2byte_ver.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ikarashiMDC_2byte_ver.lib Mon Oct 10 08:01:10 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/ikarashiMDC_2byte_ver/#97bb662f1e1f
diff -r 000000000000 -r b7dbdc0b19f3 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Oct 10 08:01:10 2022 +0000
@@ -0,0 +1,206 @@
+#include "mbed.h"
+#include "ikarashiMDC.h"
+#include "FEP_RX22.h"
+#include "pinconfig.h"
+#include "omni_wheel.h"
+#include "math.h"
+#include "OmniPosition.h"
+#include "PID.h"
+
+#define PI 3.141592653589
+#define maxSpeed 0.4
+
+FEP_RX22 mycon(fepTX, fepRX, fepad);
+Serial pc(pcTX, pcRX, 115200);
+Serial RS485(PC_10,PC_11,115200);
+
+
+uint8_t b[16];
+int16_t stick[4];
+int16_t trigger[4];
+int16_t volume[3];
+uint8_t toggle[4];
+uint8_t timeout;
+uint8_t data[128];
+int pw;
+
+double speed[6];
+double engine[4];
+double spin_power;
+double posiX , posiY , posiTheta;
+int TargetAngle = 0;
+int StartAngle = 0;
+int sum_1 = 0;
+int sum_2 = 0;
+DigitalOut stop(PA_5);
+ikarashiMDC motor[] = {
+ ikarashiMDC(0,0,SM,&RS485),
+ ikarashiMDC(0,1,SM,&RS485),
+ ikarashiMDC(0,2,SM,&RS485),
+ ikarashiMDC(0,3,SM,&RS485),
+};
+
+OmniWheel omni(4);
+OmniPosition posi(mwTX, mwRX);
+
+PID angle(10.0, 5.0, 0.0000005, 0.01);//値はhttps://controlabo.com/pid-gain/で調整しよう！
+
+/*プロトタイプ宣言*/
+void chassis(); //足回りの制御・ジャイロ・テラターム
+void spin(double ang); //PID
+int pm(double num); //正負判定
+
+int main(void){
+ 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);
+ while(1){
+ stop = toggle[0];
+ chassis();
+ }
+
+}
+
+void chassis(){
+
+
+ /*ジャイロ読み取り*/
+ 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);
+ /*コントローラー受信*/
+ mycon.getData(data);
+
+ for (int i=0, tmp=1; i<8; i++) {
+ pw = pow((float)2,i);
+ b[i] = (int)((data[0] & tmp)/pw);
+// pc.printf("%d ", b[i]); 上のポインタの式が分からんので無理やり10進数に変える
+ sum_1 += b[i]*pow((float)2,i+1);
+ tmp *= 2;
+ }
+ pc.printf("%3d ",sum_1);
+ /*初期化*/
+ sum_1 = 0;
+
+ for (int i=8, tmp=1, j=0; i<16; i++, j++) {
+ pw = pow((float)2,j);
+ b[i] = (int)((data[1] & tmp)/pw);
+// pc.printf("%d ", b[i]);
+ sum_2 += b[i]*pow((float)2,i-7);
+ tmp *= 2;
+ }
+ pc.printf("%3d ",sum_2);
+ pc.printf(" | ");
+ /*初期化*/
+ sum_2 = 0;
+ pc.printf(" | ");
+
+ for (int i=0; i<4; i++) {
+ stick[i] = (data[i+2] - 128)*(-1);
+ pc.printf("%3d ", stick[i]);
+ }
+ pc.printf(" | ");
+
+ for (int i=0; i<2; i++) {
+ trigger[i] = data[i+6];
+ pc.printf("%3d ", trigger[i]);
+ }
+ pc.printf(" | ");
+
+ for (int i=0; i<3; i++) {
+ volume[i] = data[i+9];
+ pc.printf("%3d ", volume[i]);
+ }
+ pc.printf(" | ");
+
+ for (int i=0; i<4; i++) {
+ toggle[i] = data[i+12];
+ pc.printf("%d ", toggle[i]);
+ }
+ pc.printf(" | ");
+
+ timeout = data[8];
+ pc.printf("%3d ", timeout);
+ pc.printf(" | ");
+
+ if(mycon.getStatus()) pc.printf("received\r\n");
+ else{ pc.printf("anything error...\r\n");
+ for( int i=0; i<4; i++){
+ motor[i].setSpeed(0);
+ }
+ }
+
+ /*PID*/
+ if(abs(stick[2]) < 10){
+ /*for(int i=-180;i<=180;i+=15){
+ if(((posiTheta>=i-7.5)&&(posiTheta<i+7.5))&&((posiTheta>i+1)||(posiTheta<i-1))){
+ spin(i);
+ }
+ else if((posiTheta == i) ||(posiTheta == i+1) ||(posiTheta == i-1)){
+ }
+ }*/
+ 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){
+ engine[i] = maxSpeed*(stick[i]/128.0);
+ }else if(b[0]){
+ engine[1] = maxSpeed*(trigger[0]/225.0);
+ engine[0] = 0;
+ }else if(b[1]){
+ engine[0] = -maxSpeed*(trigger[0]/225.0);
+ engine[1] = 0;
+ }else if(b[2]){
+ engine[1] = -maxSpeed*(trigger[0]/225.0);
+ engine[0] = 0;
+ }else if(b[3]){
+ engine[0] = maxSpeed*(trigger[0]/255.0);
+ engine[1] = 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);
+ //for(int i=4; i<12; i++) motor[i].setSpeed(0);射出機構とかのモーターが出てきたときに[//]を消す
+ pc.printf("ang:%.2f pid:%.2f posi:%.2f T-p:%.2f\r\n",ang,-angle.compute(),posiTheta,TargetAngle-posiTheta);
+
+}
+int pm(double num){
+ return((num>0)-(num<0));
+}
+
\ No newline at end of file
diff -r 000000000000 -r b7dbdc0b19f3 mbed-os.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-os.lib Mon Oct 10 08:01:10 2022 +0000 @@ -0,0 +1,1 @@ +https://github.com/ARMmbed/mbed-os/#f8b140f8d7cb226e41486c5df66ac4f3ce699219
diff -r 000000000000 -r b7dbdc0b19f3 ommi_wheel.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ommi_wheel.lib Mon Oct 10 08:01:10 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/omni_wheel/#cfec945ea421
diff -r 000000000000 -r b7dbdc0b19f3 pinconfig.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pinconfig.h Mon Oct 10 08:01:10 2022 +0000 @@ -0,0 +1,18 @@ +#ifndef PIN_CONFIG_H +#define PIN_CONFIG_H + +#include "mbed.h" + +/*FEP*/ +static PinName const fepTX = PC_12; +static PinName const fepRX = PD_2; +static uint8_t const fepad = 021;// 通信相手のアドレス + +/*PC*/ +static PinName const pcTX = USBTX; +static PinName const pcRX = USBRX; + +/*measuring wheelシリアルマルチバイト*/ +static PinName const mwTX = PC_6; +static PinName const mwRX = PC_7; +#endif \ No newline at end of file