Soma Takahashi / Mbed OS 2019OpenCampus

Only program of OpenCampus2019

Dependencies:   QEI omni_wheel PID R1370 sheets ikarashiMDC PS3

main.cpp@0:e6f6feb5f802, 2019-08-09 (annotated)

Committer:
ec30109b
Date:
Fri Aug 09 13:24:42 2019 +0000
Revision:
0:e6f6feb5f802
komitto

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ec30109b 0:e6f6feb5f802 1 #include "mbed.h"
ec30109b 0:e6f6feb5f802 2 #include "ikarashiMDC.h"
ec30109b 0:e6f6feb5f802 3 #include "PID.h"
ec30109b 0:e6f6feb5f802 4 #include "R1370.h"
ec30109b 0:e6f6feb5f802 5 #include "PS3.h"
ec30109b 0:e6f6feb5f802 6 #include "omni_wheel.h"
ec30109b 0:e6f6feb5f802 7 #include "sheets.h"
ec30109b 0:e6f6feb5f802 8 #define PI 3.141592653589
ec30109b 0:e6f6feb5f802 9
ec30109b 0:e6f6feb5f802 10 Serial serial(PC_6, PC_7, 115200);
ec30109b 0:e6f6feb5f802 11 Serial pc(USBTX, USBRX, 115200);
ec30109b 0:e6f6feb5f802 12 PS3 ps3(PA_0,PA_1);
ec30109b 0:e6f6feb5f802 13 OmniWheel omni(4);
ec30109b 0:e6f6feb5f802 14 ikarashiMDC motor[]=
ec30109b 0:e6f6feb5f802 15 {
ec30109b 0:e6f6feb5f802 16 ikarashiMDC(2,0,SM,&serial),
ec30109b 0:e6f6feb5f802 17 ikarashiMDC(2,1,SM,&serial),
ec30109b 0:e6f6feb5f802 18 ikarashiMDC(2,2,SM,&serial),
ec30109b 0:e6f6feb5f802 19 ikarashiMDC(2,3,SM,&serial)
ec30109b 0:e6f6feb5f802 20 };
ec30109b 0:e6f6feb5f802 21 R1370 gyro(PB_6,PA_10);
ec30109b 0:e6f6feb5f802 22 PID pid(4.0, 0, 0.001, 0.01);
ec30109b 0:e6f6feb5f802 23 DigitalOut hizyou1(PC_2);
ec30109b 0:e6f6feb5f802 24 DigitalOut hizyou2(PC_3);
ec30109b 0:e6f6feb5f802 25 sheets sheets(&serial);
ec30109b 0:e6f6feb5f802 26 int state1=0,state2=0,state3=0;
ec30109b 0:e6f6feb5f802 27
ec30109b 0:e6f6feb5f802 28 double sign(double a){
ec30109b 0:e6f6feb5f802 29 return(a>0)-(a<0);
ec30109b 0:e6f6feb5f802 30 }
ec30109b 0:e6f6feb5f802 31
ec30109b 0:e6f6feb5f802 32 int main()
ec30109b 0:e6f6feb5f802 33 {
ec30109b 0:e6f6feb5f802 34 serial.baud(115200);
ec30109b 0:e6f6feb5f802 35 for(int i=0;i<4;i++)motor[i].braking = true;
ec30109b 0:e6f6feb5f802 36
ec30109b 0:e6f6feb5f802 37 bool b[11]={}, b2[11]={}, b3[11]={};
ec30109b 0:e6f6feb5f802 38 int angle_state;
ec30109b 0:e6f6feb5f802 39 double rad[2], dis[2], value[3],SPe[4], X, Y, original_angle=0, now_angle, start_angle, zero=0, spin_power;
ec30109b 0:e6f6feb5f802 40 double deviation = 0.08;
ec30109b 0:e6f6feb5f802 41
ec30109b 0:e6f6feb5f802 42 hizyou1 = 1;
ec30109b 0:e6f6feb5f802 43 hizyou2 = 1;
ec30109b 0:e6f6feb5f802 44
ec30109b 0:e6f6feb5f802 45 pid.setInputLimits(-180,180);
ec30109b 0:e6f6feb5f802 46 pid.setOutputLimits(-0.3,0.3);
ec30109b 0:e6f6feb5f802 47 pid.setBias(0);
ec30109b 0:e6f6feb5f802 48 pid.setMode(1);
ec30109b 0:e6f6feb5f802 49 pid.setSetPoint(0);
ec30109b 0:e6f6feb5f802 50 omni.wheel[0].setRadian(PI * 1.0 / 4.0);
ec30109b 0:e6f6feb5f802 51 omni.wheel[1].setRadian(PI * 3.0 / 4.0);
ec30109b 0:e6f6feb5f802 52 omni.wheel[2].setRadian(PI * 5.0 / 4.0);
ec30109b 0:e6f6feb5f802 53 omni.wheel[3].setRadian(PI * 7.0 / 4.0);
ec30109b 0:e6f6feb5f802 54
ec30109b 0:e6f6feb5f802 55 while(true){
ec30109b 0:e6f6feb5f802 56 //PS3 値読み取り
ec30109b 0:e6f6feb5f802 57 int stick[4],trigger[2];
ec30109b 0:e6f6feb5f802 58
ec30109b 0:e6f6feb5f802 59 /*ボタンスイッチ*/
ec30109b 0:e6f6feb5f802 60 for(int i = 0; i < 12; i++) {
ec30109b 0:e6f6feb5f802 61 b[i] = ps3.getButton(i);
ec30109b 0:e6f6feb5f802 62 pc.printf("%2d", b[i] );
ec30109b 0:e6f6feb5f802 63 }
ec30109b 0:e6f6feb5f802 64 /*ジョイスティック*/
ec30109b 0:e6f6feb5f802 65 for(int i = 0; i < 4; i++) {
ec30109b 0:e6f6feb5f802 66 stick[i] = ps3.getStick(i);
ec30109b 0:e6f6feb5f802 67 pc.printf("%4d", stick[i] );
ec30109b 0:e6f6feb5f802 68 }
ec30109b 0:e6f6feb5f802 69 /*トリガースイッチ*/
ec30109b 0:e6f6feb5f802 70 for(int i = 0; i < 2; i++) {
ec30109b 0:e6f6feb5f802 71 trigger[i] = ps3.getTrigger(i);
ec30109b 0:e6f6feb5f802 72 pc.printf("%4d",trigger[i]);
ec30109b 0:e6f6feb5f802 73 }
ec30109b 0:e6f6feb5f802 74
ec30109b 0:e6f6feb5f802 75 /**
ec30109b 0:e6f6feb5f802 76 * now_angle : 今の角度
ec30109b 0:e6f6feb5f802 77 * start_angle : 素の角度
ec30109b 0:e6f6feb5f802 78 * original_angle : 0,-90,90,180
ec30109b 0:e6f6feb5f802 79 * deviation : 差
ec30109b 0:e6f6feb5f802 80 * angle_state : 90度毎
ec30109b 0:e6f6feb5f802 81 * zero : 零点合わせ
ec30109b 0:e6f6feb5f802 82 */
ec30109b 0:e6f6feb5f802 83
ec30109b 0:e6f6feb5f802 84
ec30109b 0:e6f6feb5f802 85 X = stick[2];
ec30109b 0:e6f6feb5f802 86 Y = stick[3];
ec30109b 0:e6f6feb5f802 87 gyro.update();
ec30109b 0:e6f6feb5f802 88 for (int i = 0; i < 12; i++) {
ec30109b 0:e6f6feb5f802 89 if(b[i] == true && b2[i] == false){
ec30109b 0:e6f6feb5f802 90 b3[i] = !b3[i];
ec30109b 0:e6f6feb5f802 91 }
ec30109b 0:e6f6feb5f802 92 }
ec30109b 0:e6f6feb5f802 93 for (int i = 0; i < 12; i++) b2[i] = b[i];
ec30109b 0:e6f6feb5f802 94 /**
ec30109b 0:e6f6feb5f802 95 * ここまでテンプル
ec30109b 0:e6f6feb5f802 96 */
ec30109b 0:e6f6feb5f802 97 start_angle = gyro.getAngle();
ec30109b 0:e6f6feb5f802 98
ec30109b 0:e6f6feb5f802 99 if ( b3[9] == 1) angle_state++;
ec30109b 0:e6f6feb5f802 100 if ( b3[7] == 1) angle_state--;
ec30109b 0:e6f6feb5f802 101 angle_state %= 4;
ec30109b 0:e6f6feb5f802 102 switch( ( angle_state + 8 ) % 4 ){
ec30109b 0:e6f6feb5f802 103 case 1: original_angle=180;
ec30109b 0:e6f6feb5f802 104 break;
ec30109b 0:e6f6feb5f802 105 case 2: original_angle=-90;
ec30109b 0:e6f6feb5f802 106 break;
ec30109b 0:e6f6feb5f802 107 case 3: original_angle=0;
ec30109b 0:e6f6feb5f802 108 break;
ec30109b 0:e6f6feb5f802 109 default : original_angle=90;
ec30109b 0:e6f6feb5f802 110 break;
ec30109b 0:e6f6feb5f802 111 }
ec30109b 0:e6f6feb5f802 112 if ( b[6] ) original_angle = start_angle;
ec30109b 0:e6f6feb5f802 113
ec30109b 0:e6f6feb5f802 114 now_angle = start_angle - original_angle;
ec30109b 0:e6f6feb5f802 115
ec30109b 0:e6f6feb5f802 116 if ( now_angle > 180 ) now_angle = now_angle - 360;
ec30109b 0:e6f6feb5f802 117 if ( now_angle < -180 ) now_angle = now_angle + 360;
ec30109b 0:e6f6feb5f802 118
ec30109b 0:e6f6feb5f802 119 pid.setProcessValue(now_angle);
ec30109b 0:e6f6feb5f802 120 /**
ec30109b 0:e6f6feb5f802 121 * 定型文
ec30109b 0:e6f6feb5f802 122 */
ec30109b 0:e6f6feb5f802 123 spin_power = pid.compute();
ec30109b 0:e6f6feb5f802 124 X = (X - 127)/127;
ec30109b 0:e6f6feb5f802 125 Y = (Y - 127)/127;
ec30109b 0:e6f6feb5f802 126 omni.computeXY(X,Y,-spin_power);
ec30109b 0:e6f6feb5f802 127 pc.printf("%.3f||%.3f||%.3f||%.3f||%.3f||%d\n\r",X, Y, start_angle, now_angle, spin_power, angle_state);
ec30109b 0:e6f6feb5f802 128 for(int i = 0;i<4;i++){
ec30109b 0:e6f6feb5f802 129 SPe[i] = omni.wheel[i];
ec30109b 0:e6f6feb5f802 130 }
ec30109b 0:e6f6feb5f802 131 if(fabs(SPe[0]) < 0.08)SPe[0]=0;
ec30109b 0:e6f6feb5f802 132 if(fabs(SPe[1]) < 0.08)SPe[1]=0;
ec30109b 0:e6f6feb5f802 133 if(fabs(SPe[2]) < 0.08)SPe[2]=0;
ec30109b 0:e6f6feb5f802 134 if(fabs(SPe[3]) < 0.08)SPe[3]=0;
ec30109b 0:e6f6feb5f802 135
ec30109b 0:e6f6feb5f802 136 for(int i = 0; i < 4; i++)motor[i].setSpeed(SPe[i]*0.5);
ec30109b 0:e6f6feb5f802 137
ec30109b 0:e6f6feb5f802 138
ec30109b 0:e6f6feb5f802 139 //シーツ機構
ec30109b 0:e6f6feb5f802 140 if(b3[0] == true && b3[2] ==false && b3[3] ==false){
ec30109b 0:e6f6feb5f802 141 sheets.lift();
ec30109b 0:e6f6feb5f802 142 }
ec30109b 0:e6f6feb5f802 143 else if(b3[0] ==false && b3[2] ==true && b3[3] ==false){
ec30109b 0:e6f6feb5f802 144 sheets.descent();
ec30109b 0:e6f6feb5f802 145 }
ec30109b 0:e6f6feb5f802 146 else if(b3[0] ==false && b3[2] ==false && b3[3] ==true){
ec30109b 0:e6f6feb5f802 147 sheets.air_open();
ec30109b 0:e6f6feb5f802 148 }
ec30109b 0:e6f6feb5f802 149 else{
ec30109b 0:e6f6feb5f802 150 sheets.air_close();
ec30109b 0:e6f6feb5f802 151 }
ec30109b 0:e6f6feb5f802 152 for(int i=0;i<12;i++)pc.printf("%d ",b3[i]);
ec30109b 0:e6f6feb5f802 153
ec30109b 0:e6f6feb5f802 154 }
ec30109b 0:e6f6feb5f802 155 }
ec30109b 0:e6f6feb5f802 156
ec30109b 0:e6f6feb5f802 157