![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
version which is easier to read
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?
User | Revision | Line number | New 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 | } |