Soma Takahashi
Only program of OpenCampus2019
Dependencies: QEI omni_wheel PID R1370 sheets ikarashiMDC PS3
Revision 0:e6f6feb5f802, committed 2019-08-09
- Comitter:
- ec30109b
- Date:
- Fri Aug 09 13:24:42 2019 +0000
- Commit message:
- komitto
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Fri Aug 09 13:24:42 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/aberk/code/PID/#6e12a3e5af19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PS3.lib Fri Aug 09 13:24:42 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/PS3/#78827486d24f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Fri Aug 09 13:24:42 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/QEI/#fe23b32e62ca
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/R1370.lib Fri Aug 09 13:24:42 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/tknara/code/R1370/#96f91d9e3bff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ikarashiMDC.lib Fri Aug 09 13:24:42 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/ec30109b/code/ikarashiMDC/#1e4b431e711a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Aug 09 13:24:42 2019 +0000
@@ -0,0 +1,157 @@
+#include "mbed.h"
+#include "ikarashiMDC.h"
+#include "PID.h"
+#include "R1370.h"
+#include "PS3.h"
+#include "omni_wheel.h"
+#include "sheets.h"
+#define PI 3.141592653589
+
+Serial serial(PC_6, PC_7, 115200);
+Serial pc(USBTX, USBRX, 115200);
+PS3 ps3(PA_0,PA_1);
+OmniWheel omni(4);
+ikarashiMDC motor[]=
+{
+ ikarashiMDC(2,0,SM,&serial),
+ ikarashiMDC(2,1,SM,&serial),
+ ikarashiMDC(2,2,SM,&serial),
+ ikarashiMDC(2,3,SM,&serial)
+};
+R1370 gyro(PB_6,PA_10);
+PID pid(4.0, 0, 0.001, 0.01);
+DigitalOut hizyou1(PC_2);
+DigitalOut hizyou2(PC_3);
+sheets sheets(&serial);
+int state1=0,state2=0,state3=0;
+
+double sign(double a){
+ return(a>0)-(a<0);
+}
+
+int main()
+{
+ serial.baud(115200);
+ for(int i=0;i<4;i++)motor[i].braking = true;
+
+ bool b[11]={}, b2[11]={}, b3[11]={};
+ int angle_state;
+ double rad[2], dis[2], value[3],SPe[4], X, Y, original_angle=0, now_angle, start_angle, zero=0, spin_power;
+ double deviation = 0.08;
+
+ hizyou1 = 1;
+ hizyou2 = 1;
+
+ pid.setInputLimits(-180,180);
+ pid.setOutputLimits(-0.3,0.3);
+ pid.setBias(0);
+ pid.setMode(1);
+ pid.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);
+
+ while(true){
+ //PS3 値読み取り
+ int stick[4],trigger[2];
+
+ /*ボタンスイッチ*/
+ for(int i = 0; i < 12; i++) {
+ b[i] = ps3.getButton(i);
+ pc.printf("%2d", b[i] );
+ }
+ /*ジョイスティック*/
+ for(int i = 0; i < 4; i++) {
+ stick[i] = ps3.getStick(i);
+ pc.printf("%4d", stick[i] );
+ }
+ /*トリガースイッチ*/
+ for(int i = 0; i < 2; i++) {
+ trigger[i] = ps3.getTrigger(i);
+ pc.printf("%4d",trigger[i]);
+ }
+
+ /**
+ * now_angle : 今の角度
+ * start_angle : 素の角度
+ * original_angle : 0,-90,90,180
+ * deviation : 差
+ * angle_state : 90度毎
+ * zero : 零点合わせ
+ */
+
+
+ X = stick[2];
+ Y = stick[3];
+ gyro.update();
+ for (int i = 0; i < 12; i++) {
+ if(b[i] == true && b2[i] == false){
+ b3[i] = !b3[i];
+ }
+ }
+ for (int i = 0; i < 12; i++) b2[i] = b[i];
+ /**
+ * ここまでテンプル
+ */
+ start_angle = gyro.getAngle();
+
+ if ( b3[9] == 1) angle_state++;
+ if ( b3[7] == 1) angle_state--;
+ angle_state %= 4;
+ switch( ( angle_state + 8 ) % 4 ){
+ case 1: original_angle=180;
+ break;
+ case 2: original_angle=-90;
+ break;
+ case 3: original_angle=0;
+ break;
+ default : original_angle=90;
+ break;
+ }
+ if ( b[6] ) original_angle = start_angle;
+
+ now_angle = start_angle - original_angle;
+
+ if ( now_angle > 180 ) now_angle = now_angle - 360;
+ if ( now_angle < -180 ) now_angle = now_angle + 360;
+
+ pid.setProcessValue(now_angle);
+ /**
+ * 定型文
+ */
+ spin_power = pid.compute();
+ X = (X - 127)/127;
+ Y = (Y - 127)/127;
+ omni.computeXY(X,Y,-spin_power);
+ pc.printf("%.3f||%.3f||%.3f||%.3f||%.3f||%d\n\r",X, Y, start_angle, now_angle, spin_power, angle_state);
+ for(int i = 0;i<4;i++){
+ SPe[i] = omni.wheel[i];
+ }
+ if(fabs(SPe[0]) < 0.08)SPe[0]=0;
+ if(fabs(SPe[1]) < 0.08)SPe[1]=0;
+ if(fabs(SPe[2]) < 0.08)SPe[2]=0;
+ if(fabs(SPe[3]) < 0.08)SPe[3]=0;
+
+ for(int i = 0; i < 4; i++)motor[i].setSpeed(SPe[i]*0.5);
+
+
+ //シーツ機構
+ if(b3[0] == true && b3[2] ==false && b3[3] ==false){
+ sheets.lift();
+ }
+ else if(b3[0] ==false && b3[2] ==true && b3[3] ==false){
+ sheets.descent();
+ }
+ else if(b3[0] ==false && b3[2] ==false && b3[3] ==true){
+ sheets.air_open();
+ }
+ else{
+ sheets.air_close();
+ }
+ for(int i=0;i<12;i++)pc.printf("%d ",b3[i]);
+
+ }
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-os.lib Fri Aug 09 13:24:42 2019 +0000 @@ -0,0 +1,1 @@ +https://github.com/ARMmbed/mbed-os/#b81aeff1a3e171c6421984faa2cc18d0e35746c0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/omni_wheel.lib Fri Aug 09 13:24:42 2019 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/omni_wheel/#cfec945ea421
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/sheets.lib Fri Aug 09 13:24:42 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/ec30109b/code/sheets/#9febfe55fa90