春ロボ　ロケット団 / Mbed 2 deprecated spring_ps3_2

test

Dependencies:   mbed KondoServoLibrary Encoder

User.cpp@19:510b87211851, 2020-03-11 (annotated)

Committer:
o2132613
Date:
Wed Mar 11 09:08:05 2020 +0000
Revision:
19:510b87211851
Parent:
18:f7c587f0f8b8
Child:
20:dd4634500bb3
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hirokimineshita 0:736c76a75def 1 #include "Utils.h"
hirokimineshita 0:736c76a75def 2 #include "USBHost.h"
hirokimineshita 0:736c76a75def 3 #include "hci.h"
hirokimineshita 0:736c76a75def 4 #include "ps3.h"
hirokimineshita 0:736c76a75def 5 #include "User.h"
o2132613 19:510b87211851 6 #include "mbed.h"
o2132613 19:510b87211851 7 #include "math.h"
hirokimineshita 0:736c76a75def 8 #include "mbed.h"
o2132613 19:510b87211851 9 #include"KondoServo.h"
o2132613 19:510b87211851 10 #include "EC.h" //Encoderライブラリをインクルード
o2132613 19:510b87211851 11 #define RESOLUTION 500
hirokimineshita 0:736c76a75def 12
hirokimineshita 0:736c76a75def 13 int RSX,RSY,LSX,LSY,BSU,BSL;
o2132613 19:510b87211851 14 //Digitalout cs(p8);
o2132613 19:510b87211851 15 int move=5,shoot=0,ball=0,box=0;
o2132613 19:510b87211851 16 CAN controller(p30,p29);//CANpin_name
o2132613 19:510b87211851 17 DigitalOut led1(LED1);
o2132613 19:510b87211851 18 DigitalOut led2(LED2);
o2132613 19:510b87211851 19 DigitalOut led3(LED3);
o2132613 19:510b87211851 20 DigitalOut led4(LED4);
o2132613 19:510b87211851 21 char can[1]= {0};
o2132613 19:510b87211851 22 KondoServo servo(p28,p27);//サーボ
o2132613 19:510b87211851 23 int out = 1; // shot
o2132613 19:510b87211851 24
o2132613 19:510b87211851 25
o2132613 19:510b87211851 26 PwmOut f(p26);//投F
o2132613 19:510b87211851 27 PwmOut b(p25);//投B
o2132613 19:510b87211851 28 Ticker ticker;
o2132613 19:510b87211851 29 Ec1multi EC(p16,p17,RESOLUTION);//投E
o2132613 19:510b87211851 30 InterruptIn X(p15);
o2132613 19:510b87211851 31 DigitalOut out1(p19);//エアシリンダー（つかむ方）
o2132613 19:510b87211851 32 DigitalOut out2(p20);//エアシリンダー（投擲）
o2132613 17:348c660ea6f6 33
baba2357 11:86d717718dbf 34
o2132613 19:510b87211851 35 int id = 0;
o2132613 19:510b87211851 36 double SERVO2DEG = 270.0 / (11500 - 3500);
o2132613 19:510b87211851 37 double first = (6800 - 3500) * SERVO2DEG;
o2132613 19:510b87211851 38 double grab = (3800 - 3500) * SERVO2DEG;
o2132613 19:510b87211851 39 double pass = (5000 - 3500) * SERVO2DEG;
o2132613 19:510b87211851 40
o2132613 19:510b87211851 41 void cal()
o2132613 19:510b87211851 42 {
o2132613 19:510b87211851 43 EC.calOmega();
o2132613 19:510b87211851 44 }
o2132613 19:510b87211851 45 void shot();
o2132613 19:510b87211851 46 void catch_ball();
o2132613 19:510b87211851 47
o2132613 19:510b87211851 48 int X_count=0;
o2132613 19:510b87211851 49 void Xcount()
o2132613 19:510b87211851 50 {
o2132613 19:510b87211851 51 X_count++;
o2132613 19:510b87211851 52 }
o2132613 19:510b87211851 53
o2132613 17:348c660ea6f6 54 void UserLoopSetting()
o2132613 17:348c660ea6f6 55 {
hirokimineshita 0:736c76a75def 56 }
o2132613 17:348c660ea6f6 57 void UserLoop(char n,const u8* data)
o2132613 17:348c660ea6f6 58 {
hirokimineshita 0:736c76a75def 59 u16 ButtonState;
o2132613 17:348c660ea6f6 60 if(n==0) { //有線Ps3USB.cpp
hirokimineshita 0:736c76a75def 61 RSX = ((ps3report*)data)->RightStickX;
hirokimineshita 0:736c76a75def 62 RSY = ((ps3report*)data)->RightStickY;
hirokimineshita 0:736c76a75def 63 LSX = ((ps3report*)data)->LeftStickX;
hirokimineshita 0:736c76a75def 64 LSY = ((ps3report*)data)->LeftStickY;
hirokimineshita 0:736c76a75def 65 BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
hirokimineshita 0:736c76a75def 66 BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
hirokimineshita 0:736c76a75def 67 //ボタンの処理
hirokimineshita 0:736c76a75def 68 ButtonState = ((ps3report*)data)->ButtonState;
o2132613 17:348c660ea6f6 69 } else {//無線TestShell.cpp
hirokimineshita 0:736c76a75def 70 RSX = ((ps3report*)(data + 1))->RightStickX;
hirokimineshita 0:736c76a75def 71 RSY = ((ps3report*)(data + 1))->RightStickY;
hirokimineshita 0:736c76a75def 72 LSX = ((ps3report*)(data + 1))->LeftStickX;
hirokimineshita 0:736c76a75def 73 LSY = ((ps3report*)(data + 1))->LeftStickY;
hirokimineshita 0:736c76a75def 74 BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
hirokimineshita 0:736c76a75def 75 BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
hirokimineshita 0:736c76a75def 76 //ボタンの処理
hirokimineshita 0:736c76a75def 77 ButtonState = ((ps3report*)(data + 1))->ButtonState;
hirokimineshita 0:736c76a75def 78 }
hirokimineshita 0:736c76a75def 79 //ここより下にプログラムを書く
o2132613 19:510b87211851 80 controller.frequency(1000000);
o2132613 19:510b87211851 81 led1=0;
o2132613 19:510b87211851 82 led2=0;
o2132613 19:510b87211851 83 led3=0;
o2132613 19:510b87211851 84 led4=0;
o2132613 19:510b87211851 85
o2132613 19:510b87211851 86 //move
o2132613 19:510b87211851 87 if((ButtonState >> BUTTONUP)&1 == 1&&(ButtonState >> BUTTONL1)&1 == 1) { //UPL1
o2132613 19:510b87211851 88 move = 9;
o2132613 19:510b87211851 89 } else if ((ButtonState >> BUTTONDOWN)&1 == 1&&(ButtonState >> BUTTONR1)&1 == 1) { //DownR1
o2132613 19:510b87211851 90 move = 3;
o2132613 19:510b87211851 91 } else if ((ButtonState >> BUTTONUP)&1 == 1&&(ButtonState >> BUTTONR1)&1 == 1) { //UPR1
o2132613 19:510b87211851 92 move = 7;
o2132613 19:510b87211851 93 } else if ((ButtonState >> BUTTONDOWN)&1 == 1&&(ButtonState >> BUTTONL1)&1 == 1) { //DownL1
o2132613 19:510b87211851 94 move = 1;
o2132613 19:510b87211851 95 } else if((ButtonState >> BUTTONUP)&1 == 1) { //up
o2132613 19:510b87211851 96 move = 8;
o2132613 19:510b87211851 97 led1 = 1;
o2132613 19:510b87211851 98 } else if((ButtonState >> BUTTONDOWN)&1 == 1) { //down
o2132613 19:510b87211851 99 move = 2;
o2132613 19:510b87211851 100 led2 =1;
o2132613 19:510b87211851 101 } else if((ButtonState >> BUTTONRIGHT)&1 == 1) { //right
o2132613 19:510b87211851 102 move = 6;
o2132613 19:510b87211851 103 led3 = 1;
o2132613 19:510b87211851 104 } else if((ButtonState >> BUTTONLEFT)&1 == 1) { //left
o2132613 19:510b87211851 105 move = 4;
o2132613 19:510b87211851 106 led4 = 1;
o2132613 19:510b87211851 107 } else if((ButtonState >> BUTTONL1)&1 == 1) { //L1
o2132613 19:510b87211851 108 move = 10;
o2132613 19:510b87211851 109 } else if((ButtonState >> BUTTONR1)&1 == 1) { //R1
o2132613 19:510b87211851 110 move = 11;
o2132613 19:510b87211851 111 } else if((ButtonState >> BUTTONSELECT)&1 == 1 ) {//対応するボタンを書く(今回SELLECTボタン
o2132613 19:510b87211851 112 move = 1;
o2132613 19:510b87211851 113 } else {
o2132613 19:510b87211851 114 move = 5;
o2132613 19:510b87211851 115 }
o2132613 19:510b87211851 116 //move
o2132613 19:510b87211851 117 //shoot
o2132613 19:510b87211851 118 if((ButtonState >> BUTTONCIRCLE)&1 == 1) {//O
o2132613 19:510b87211851 119 shoot = 1;
o2132613 19:510b87211851 120 led1 = 1;
o2132613 19:510b87211851 121 } else if((ButtonState >> BUTTONCROSS)&1 == 1) {//X
o2132613 19:510b87211851 122 shoot = 0;
o2132613 19:510b87211851 123 led2 = 1;
o2132613 19:510b87211851 124 } else if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//△
o2132613 19:510b87211851 125 shoot = 3;
o2132613 19:510b87211851 126 led3 = 1;
o2132613 19:510b87211851 127 } else if((ButtonState >> BUTTONSQUARE)&1 == 1) {//□
o2132613 19:510b87211851 128 shoot = 4;
o2132613 19:510b87211851 129 led4 = 1;
o2132613 17:348c660ea6f6 130 }
o2132613 19:510b87211851 131 //CAN通信用プログラム
o2132613 19:510b87211851 132 if(shoot==1)shot();
o2132613 19:510b87211851 133 if(shoot>=2)catch_ball();
o2132613 19:510b87211851 134 can[0] = move;
o2132613 19:510b87211851 135 controller.write(CANMessage(1,can,1));
o2132613 19:510b87211851 136 printf("%d\r\n",can[0]);
o2132613 19:510b87211851 137 }
o2132613 19:510b87211851 138
o2132613 19:510b87211851 139 void shot()
o2132613 19:510b87211851 140 {
o2132613 19:510b87211851 141 X.rise(&Xcount);
o2132613 19:510b87211851 142 f.period_us(50);
o2132613 19:510b87211851 143 b.period_us(50);
o2132613 19:510b87211851 144 double a=0,r=0.4,v=0;
o2132613 19:510b87211851 145 int i=0,count;//rで半径指定 a*r=v
o2132613 19:510b87211851 146 ticker.attach(&cal,0.05);
o2132613 19:510b87211851 147 printf("set");
o2132613 19:510b87211851 148 if(X_count ==1) {
o2132613 19:510b87211851 149 EC.reset();
o2132613 19:510b87211851 150 X_count =0;
o2132613 17:348c660ea6f6 151 }
o2132613 19:510b87211851 152 //角度リセット
o2132613 19:510b87211851 153 while(1) {
o2132613 19:510b87211851 154 printf("%.3f %.3f %d %d\r\n",a,v,X_count,i);
o2132613 19:510b87211851 155 a=EC.getOmega();
o2132613 19:510b87211851 156 count=EC.getCount();
o2132613 19:510b87211851 157 i =count%500;
o2132613 19:510b87211851 158 v=a*r;
o2132613 19:510b87211851 159 f=0.45;//速度一定
o2132613 19:510b87211851 160 b=0.0;
o2132613 19:510b87211851 161 led1=1;
o2132613 19:510b87211851 162 led2=0;
o2132613 19:510b87211851 163 led3=0;
o2132613 19:510b87211851 164 led4=1;
o2132613 19:510b87211851 165 if(X_count>3) {
o2132613 19:510b87211851 166 if(i < -380 && i > -390) {//離す角度を決める
o2132613 19:510b87211851 167 out2=1; //ボールを離す
o2132613 19:510b87211851 168 printf("shot\r\n");
o2132613 19:510b87211851 169 led1=1;
o2132613 19:510b87211851 170 led2=1;
o2132613 19:510b87211851 171 led3=1;
o2132613 19:510b87211851 172 led4=1;
o2132613 19:510b87211851 173 break;
o2132613 19:510b87211851 174 }
o2132613 19:510b87211851 175 }
o2132613 17:348c660ea6f6 176 }
o2132613 19:510b87211851 177 printf("%.3f %.3f %d %d stop\r\n",a,v,X_count,i);
o2132613 19:510b87211851 178 X.rise(&Xcount);
o2132613 19:510b87211851 179 a=EC.getOmega();
o2132613 19:510b87211851 180 count=EC.getCount();
o2132613 19:510b87211851 181 i =count%500;
o2132613 19:510b87211851 182 f=0;
o2132613 19:510b87211851 183 b=0;
o2132613 19:510b87211851 184 led1=0;
o2132613 19:510b87211851 185 led2=1;
o2132613 19:510b87211851 186 led3=1;
o2132613 19:510b87211851 187 led4=0;
o2132613 19:510b87211851 188
o2132613 19:510b87211851 189 }
o2132613 19:510b87211851 190
o2132613 19:510b87211851 191 void catch_ball()
o2132613 19:510b87211851 192 {
o2132613 19:510b87211851 193 switch (shoot) {
o2132613 19:510b87211851 194 case 2:
o2132613 19:510b87211851 195 servo.set_degree(id, pass);
o2132613 19:510b87211851 196 break;
o2132613 19:510b87211851 197 case 3:
o2132613 19:510b87211851 198 servo.set_degree(id, grab);
o2132613 19:510b87211851 199 break;
o2132613 19:510b87211851 200 case 4:
o2132613 19:510b87211851 201 if (out1==0){
o2132613 19:510b87211851 202 out1=1;
o2132613 19:510b87211851 203 }else if (out1 == 1){
o2132613 19:510b87211851 204 out1=0;
o2132613 19:510b87211851 205 }
o2132613 19:510b87211851 206 }
o2132613 19:510b87211851 207 }
o2132613 19:510b87211851 208
o2132613 19:510b87211851 209 void catch_box()
o2132613 19:510b87211851 210 {
o2132613 19:510b87211851 211
hirokimineshita 0:736c76a75def 212 }