kashiko aoyama / Mbed 2 deprecated r_test1

r_test1_2017_10_11_Wed_A

Dependencies:   QEI mbed

main.cpp@1:28488ffaa585, 2017-10-11 (annotated)

Committer:
shobonwarrior
Date:
Wed Oct 11 08:18:10 2017 +0000
Revision:
1:28488ffaa585
Parent:
0:996353c69d55 
r_test1_2017_10_11_Wed_A

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shobonwarrior 0:996353c69d55 1 #include "mbed.h"
shobonwarrior 0:996353c69d55 2 #include "QEI.h"
shobonwarrior 0:996353c69d55 3 #include "ps3CTRLmbed.h"
shobonwarrior 0:996353c69d55 4 /*
shobonwarrior 0:996353c69d55 5 上　下
shobonwarrior 0:996353c69d55 6 R1　L1　０．５
shobonwarrior 0:996353c69d55 7 R2　L2　遅く
shobonwarrior 0:996353c69d55 8 立ち上がりで停止 pwm=0
shobonwarrior 0:996353c69d55 9 〇　4段階　　2個　　０にする
shobonwarrior 0:996353c69d55 10 × 停止
shobonwarrior 0:996353c69d55 11 □ 0 1
shobonwarrior 0:996353c69d55 12 △ 1 0
shobonwarrior 0:996353c69d55 13 */
shobonwarrior 0:996353c69d55 14 void RL_ONE(void);
shobonwarrior 0:996353c69d55 15 void RL_TWO(void);
shobonwarrior 0:996353c69d55 16
shobonwarrior 0:996353c69d55 17 QEI wheel1(p30, p29, NC, 624);//p24,p19のモーター
shobonwarrior 0:996353c69d55 18 QEI wheel2(p7, p8, NC, 624);//p25,p20のモーター
shobonwarrior 0:996353c69d55 19 Serial pc(USBTX, USBRX); // tx, rx
shobonwarrior 0:996353c69d55 20
shobonwarrior 0:996353c69d55 21 void QEI(void);
shobonwarrior 0:996353c69d55 22
shobonwarrior 0:996353c69d55 23 InterruptIn PH1(p12);//L1とR1のアレ
shobonwarrior 0:996353c69d55 24 InterruptIn PH2(p13);//L2とR2のアレ
shobonwarrior 0:996353c69d55 25
shobonwarrior 0:996353c69d55 26 /*100行目あたり。□かつS1＝０で動く。△かつRS2=0で動く。*/
shobonwarrior 0:996353c69d55 27 DigitalIn RS1(p5);
shobonwarrior 0:996353c69d55 28 DigitalIn RS2(p6);
shobonwarrior 0:996353c69d55 29
shobonwarrior 0:996353c69d55 30 DigitalOut ST1(p14);
shobonwarrior 0:996353c69d55 31 DigitalOut ST2(p15);
shobonwarrior 0:996353c69d55 32 DigitalOut UD(p16);
shobonwarrior 0:996353c69d55 33 DigitalOut RL1(p17);
shobonwarrior 0:996353c69d55 34 DigitalOut RL2(p18);
shobonwarrior 0:996353c69d55 35 DigitalOut C1(p19);
shobonwarrior 0:996353c69d55 36 DigitalOut C2(p20);
shobonwarrior 0:996353c69d55 37
shobonwarrior 0:996353c69d55 38 PwmOut UDP(p21);
shobonwarrior 0:996353c69d55 39 PwmOut RLP1(p22);
shobonwarrior 0:996353c69d55 40 PwmOut RLP2(p23);
shobonwarrior 0:996353c69d55 41 PwmOut CP1(p24);
shobonwarrior 0:996353c69d55 42 PwmOut CP2(p25);
shobonwarrior 0:996353c69d55 43 PwmOut STP(p26);
shobonwarrior 0:996353c69d55 44
shobonwarrior 0:996353c69d55 45 I2C i2c(p9,p10);
shobonwarrior 0:996353c69d55 46
shobonwarrior 0:996353c69d55 47 int addr=0x00;//アドレスをいじるならここ
shobonwarrior 0:996353c69d55 48 int ack=0;
shobonwarrior 0:996353c69d55 49 double t1=0;
shobonwarrior 0:996353c69d55 50 double t2=0;
shobonwarrior 0:996353c69d55 51 double t3=0;
shobonwarrior 0:996353c69d55 52 double t4=0;
shobonwarrior 0:996353c69d55 53 double t5=0;
shobonwarrior 0:996353c69d55 54 double t6=0;
shobonwarrior 0:996353c69d55 55 int flag=0;
shobonwarrior 0:996353c69d55 56 int p=0;
shobonwarrior 0:996353c69d55 57 int q=0;
shobonwarrior 0:996353c69d55 58 int k=0;
shobonwarrior 0:996353c69d55 59 int CC=0;
shobonwarrior 0:996353c69d55 60 int M=0; //目標パルス値
shobonwarrior 0:996353c69d55 61 int rs1=0;
shobonwarrior 0:996353c69d55 62 int rs2=0;
shobonwarrior 0:996353c69d55 63
shobonwarrior 0:996353c69d55 64 int main() {
shobonwarrior 0:996353c69d55 65 char cmd[4];
shobonwarrior 0:996353c69d55 66 PS3DefaultSet();
shobonwarrior 0:996353c69d55 67 while(1) {
shobonwarrior 0:996353c69d55 68 CheckInputData();
shobonwarrior 0:996353c69d55 69 for(int i=0;i<4;i++){
shobonwarrior 0:996353c69d55 70 cmd[i]=(char)psdata[i+3];
shobonwarrior 0:996353c69d55 71 }
shobonwarrior 0:996353c69d55 72 i2c.write(addr,cmd,4);
shobonwarrior 0:996353c69d55 73
shobonwarrior 0:996353c69d55 74 /*上下*/
shobonwarrior 0:996353c69d55 75 if(BTTRUE(UP)){/*上*/
shobonwarrior 0:996353c69d55 76 t1+=0.2;
shobonwarrior 0:996353c69d55 77 if(t1>1.0){
shobonwarrior 0:996353c69d55 78 t1=1.0;
shobonwarrior 0:996353c69d55 79 }
shobonwarrior 0:996353c69d55 80 UD=0;
shobonwarrior 0:996353c69d55 81 UDP=t1;
shobonwarrior 0:996353c69d55 82 }else if(BTTRUE(DOWN)){/*下*/
shobonwarrior 0:996353c69d55 83 t1+=0.2;
shobonwarrior 0:996353c69d55 84 if(t1>1.0){
shobonwarrior 0:996353c69d55 85 t1=1.0;
shobonwarrior 0:996353c69d55 86 }
shobonwarrior 0:996353c69d55 87 UD=1;
shobonwarrior 0:996353c69d55 88 UDP=t1;
shobonwarrior 0:996353c69d55 89 }else{
shobonwarrior 0:996353c69d55 90 t1-=0.2;
shobonwarrior 0:996353c69d55 91 if(t1<0){
shobonwarrior 0:996353c69d55 92 t1=0.0;
shobonwarrior 0:996353c69d55 93 }
shobonwarrior 0:996353c69d55 94 UDP=t1;
shobonwarrior 0:996353c69d55 95 }
shobonwarrior 0:996353c69d55 96
shobonwarrior 0:996353c69d55 97 rs1=RS1;
shobonwarrior 0:996353c69d55 98 rs2=RS2;
shobonwarrior 0:996353c69d55 99
shobonwarrior 0:996353c69d55 100 /*□ △*/
shobonwarrior 0:996353c69d55 101 if(BTTRUE(SQUARE)&&rs1==0){
shobonwarrior 0:996353c69d55 102 t6+=0.2;
shobonwarrior 0:996353c69d55 103 if(t6>1.0){
shobonwarrior 0:996353c69d55 104 t6=1.0;
shobonwarrior 0:996353c69d55 105 }
shobonwarrior 0:996353c69d55 106 ST1=0;
shobonwarrior 0:996353c69d55 107 STP=t6;
shobonwarrior 0:996353c69d55 108 ST2=1;
shobonwarrior 0:996353c69d55 109 STP=t6;
shobonwarrior 0:996353c69d55 110 }else if(BTTRUE(TRIANGLE)&&rs2==0){
shobonwarrior 0:996353c69d55 111 t6+=0.2;
shobonwarrior 0:996353c69d55 112 if(t6>1.0){
shobonwarrior 0:996353c69d55 113 t6=1.0;
shobonwarrior 0:996353c69d55 114 }
shobonwarrior 0:996353c69d55 115 ST1=1;
shobonwarrior 0:996353c69d55 116 STP=t6;
shobonwarrior 0:996353c69d55 117 ST2=0;
shobonwarrior 0:996353c69d55 118 STP=t6;
shobonwarrior 0:996353c69d55 119 }else{
shobonwarrior 0:996353c69d55 120 t6-=0.2;
shobonwarrior 0:996353c69d55 121 if(t6<0.0){
shobonwarrior 0:996353c69d55 122 t6=0.0;
shobonwarrior 0:996353c69d55 123 }
shobonwarrior 0:996353c69d55 124 STP=t6;
shobonwarrior 0:996353c69d55 125 }
shobonwarrior 0:996353c69d55 126
shobonwarrior 0:996353c69d55 127 //PH1.rise(&RL_ONE);
shobonwarrior 0:996353c69d55 128 //PH2.rise(&RL_TWO);
shobonwarrior 0:996353c69d55 129
shobonwarrior 0:996353c69d55 130 /*R1 L1*/
shobonwarrior 0:996353c69d55 131 if(BTTRUE(R1)){/*R1*/
shobonwarrior 0:996353c69d55 132 PH1.rise(&RL_ONE);
shobonwarrior 0:996353c69d55 133 if(p<2){
shobonwarrior 0:996353c69d55 134 t2+=0.1;
shobonwarrior 0:996353c69d55 135 if(t2>0.5){
shobonwarrior 0:996353c69d55 136 t2=0.5;
shobonwarrior 0:996353c69d55 137 }
shobonwarrior 0:996353c69d55 138 RL1=0;
shobonwarrior 0:996353c69d55 139 RLP1=t2;
shobonwarrior 0:996353c69d55 140 }else{
shobonwarrior 0:996353c69d55 141 t2=0;
shobonwarrior 0:996353c69d55 142 RLP1=0;
shobonwarrior 0:996353c69d55 143 }
shobonwarrior 0:996353c69d55 144 }else if(BTTRUE(L1)){//L1
shobonwarrior 0:996353c69d55 145 PH1.rise(&RL_ONE);
shobonwarrior 0:996353c69d55 146 if(p<2){
shobonwarrior 0:996353c69d55 147 t2+=0.1;
shobonwarrior 0:996353c69d55 148 if(t2>0.5){
shobonwarrior 0:996353c69d55 149 t2=0.5;
shobonwarrior 0:996353c69d55 150 }
shobonwarrior 0:996353c69d55 151 RL1=1;
shobonwarrior 0:996353c69d55 152 RLP1=t2;
shobonwarrior 0:996353c69d55 153 }else{
shobonwarrior 0:996353c69d55 154 t2=0;
shobonwarrior 0:996353c69d55 155 RLP1=0;
shobonwarrior 0:996353c69d55 156 }
shobonwarrior 0:996353c69d55 157 }else{
shobonwarrior 0:996353c69d55 158 t2-=0.15;
shobonwarrior 0:996353c69d55 159 if(t2<0){
shobonwarrior 0:996353c69d55 160 t2=0;
shobonwarrior 0:996353c69d55 161 }
shobonwarrior 0:996353c69d55 162 RLP1=t2;
shobonwarrior 0:996353c69d55 163 }
shobonwarrior 0:996353c69d55 164
shobonwarrior 0:996353c69d55 165 /*R2 L2*/
shobonwarrior 0:996353c69d55 166 if(BTTRUE(R2)){/*R2*/
shobonwarrior 0:996353c69d55 167 PH2.rise(&RL_TWO);
shobonwarrior 0:996353c69d55 168 if(q<2){
shobonwarrior 0:996353c69d55 169 t3+=0.1;
shobonwarrior 0:996353c69d55 170 if(t3>0.5){
shobonwarrior 0:996353c69d55 171 t3=0.5;
shobonwarrior 0:996353c69d55 172 }
shobonwarrior 0:996353c69d55 173 RL2=0;
shobonwarrior 0:996353c69d55 174 RLP2=t3;
shobonwarrior 0:996353c69d55 175 }else{
shobonwarrior 0:996353c69d55 176 t3=0;
shobonwarrior 0:996353c69d55 177 RLP2=0;
shobonwarrior 0:996353c69d55 178 }
shobonwarrior 0:996353c69d55 179 }else if(BTTRUE(L2)){/*L2*/
shobonwarrior 0:996353c69d55 180 PH2.rise(&RL_TWO);
shobonwarrior 0:996353c69d55 181 if(q<2){
shobonwarrior 0:996353c69d55 182 t3+=0.1;
shobonwarrior 0:996353c69d55 183 if(t3>0.5){
shobonwarrior 0:996353c69d55 184 t3=0.5;
shobonwarrior 0:996353c69d55 185 }
shobonwarrior 0:996353c69d55 186 RL2=1;
shobonwarrior 0:996353c69d55 187 RLP2=t3;
shobonwarrior 0:996353c69d55 188 }else{
shobonwarrior 0:996353c69d55 189 t3=0;
shobonwarrior 0:996353c69d55 190 RLP2=0;
shobonwarrior 0:996353c69d55 191 }
shobonwarrior 0:996353c69d55 192 }else{
shobonwarrior 0:996353c69d55 193 t3-=0.15;
shobonwarrior 0:996353c69d55 194 if(t3<0){
shobonwarrior 0:996353c69d55 195 t3=0;
shobonwarrior 0:996353c69d55 196 }
shobonwarrior 0:996353c69d55 197 RLP2=t3;
shobonwarrior 0:996353c69d55 198 }
shobonwarrior 0:996353c69d55 199
shobonwarrior 0:996353c69d55 200 if(p>=2 && BTFALSE(R1) && BTFALSE(L1)){
shobonwarrior 0:996353c69d55 201 p=0;
shobonwarrior 0:996353c69d55 202 }
shobonwarrior 0:996353c69d55 203
shobonwarrior 0:996353c69d55 204 if(q>=2 && BTFALSE(R2) && BTFALSE(L2)){
shobonwarrior 0:996353c69d55 205 q=0;
shobonwarrior 0:996353c69d55 206 }
shobonwarrior 0:996353c69d55 207
shobonwarrior 0:996353c69d55 208 /*
shobonwarrior 0:996353c69d55 209 //〇
shobonwarrior 0:996353c69d55 210 while(BTTRUE(CIRCLE)){
shobonwarrior 0:996353c69d55 211 if(BTFALSE(CIRCLE)){
shobonwarrior 0:996353c69d55 212 flag=flag+1;
shobonwarrior 0:996353c69d55 213 if(flag>4){
shobonwarrior 0:996353c69d55 214 flag=1;
shobonwarrior 0:996353c69d55 215 }
shobonwarrior 0:996353c69d55 216 break;
shobonwarrior 0:996353c69d55 217 }
shobonwarrior 0:996353c69d55 218 }
shobonwarrior 0:996353c69d55 219 */
shobonwarrior 0:996353c69d55 220
shobonwarrior 0:996353c69d55 221 //〇
shobonwarrior 0:996353c69d55 222 if(BTTRUE(CIRCLE)){
shobonwarrior 0:996353c69d55 223 CC=1;
shobonwarrior 0:996353c69d55 224 }
shobonwarrior 0:996353c69d55 225
shobonwarrior 0:996353c69d55 226 if(BTFALSE(CIRCLE)){
shobonwarrior 0:996353c69d55 227 if(CC==1){
shobonwarrior 0:996353c69d55 228 flag=flag+1;
shobonwarrior 0:996353c69d55 229 if(flag>4){
shobonwarrior 0:996353c69d55 230 flag=1;
shobonwarrior 0:996353c69d55 231 }
shobonwarrior 0:996353c69d55 232 }
shobonwarrior 0:996353c69d55 233 CC=0;
shobonwarrior 0:996353c69d55 234 }
shobonwarrior 0:996353c69d55 235
shobonwarrior 0:996353c69d55 236 //×
shobonwarrior 0:996353c69d55 237 if(BTTRUE(CROSS)){
shobonwarrior 0:996353c69d55 238 CC=0;
shobonwarrior 0:996353c69d55 239 flag=0;
shobonwarrior 0:996353c69d55 240 }
shobonwarrior 0:996353c69d55 241
shobonwarrior 0:996353c69d55 242 //〇 ×
shobonwarrior 0:996353c69d55 243 if(flag>0){//CC>0
shobonwarrior 0:996353c69d55 244 //if(flag==0){
shobonwarrior 0:996353c69d55 245 t4+=0.1;
shobonwarrior 0:996353c69d55 246 t5+=0.1;
shobonwarrior 0:996353c69d55 247 //}
shobonwarrior 0:996353c69d55 248 if(t4>(flag*0.2)){
shobonwarrior 0:996353c69d55 249 t4=flag*0.2;
shobonwarrior 0:996353c69d55 250 }
shobonwarrior 0:996353c69d55 251 if(t5>(flag*0.2)){
shobonwarrior 0:996353c69d55 252 t5=flag*0.2;
shobonwarrior 0:996353c69d55 253 }
shobonwarrior 0:996353c69d55 254
shobonwarrior 0:996353c69d55 255 if(t4>0.8){
shobonwarrior 0:996353c69d55 256 t4=0.8;
shobonwarrior 0:996353c69d55 257 //flag=1;
shobonwarrior 0:996353c69d55 258 }
shobonwarrior 0:996353c69d55 259 if(t5>0.8){
shobonwarrior 0:996353c69d55 260 t5=0.8;
shobonwarrior 0:996353c69d55 261 //flag=1;
shobonwarrior 0:996353c69d55 262 }
shobonwarrior 0:996353c69d55 263 C1=0;
shobonwarrior 0:996353c69d55 264 C2=0;
shobonwarrior 0:996353c69d55 265 CP1=t4;
shobonwarrior 0:996353c69d55 266 CP2=t5;
shobonwarrior 0:996353c69d55 267 QEI();
shobonwarrior 0:996353c69d55 268 }else{
shobonwarrior 0:996353c69d55 269 t4-=0.15;
shobonwarrior 0:996353c69d55 270 t5-=0.15;
shobonwarrior 0:996353c69d55 271 if(t4<0){
shobonwarrior 0:996353c69d55 272 t4=0;
shobonwarrior 0:996353c69d55 273 }
shobonwarrior 0:996353c69d55 274 if(t5<0){
shobonwarrior 0:996353c69d55 275 t5=0;
shobonwarrior 0:996353c69d55 276 }
shobonwarrior 0:996353c69d55 277 CP1=0;
shobonwarrior 0:996353c69d55 278 CP2=0;
shobonwarrior 0:996353c69d55 279 }
shobonwarrior 0:996353c69d55 280 }
shobonwarrior 0:996353c69d55 281 }
shobonwarrior 0:996353c69d55 282
shobonwarrior 0:996353c69d55 283 void RL_ONE(void){
shobonwarrior 0:996353c69d55 284 p++;
shobonwarrior 0:996353c69d55 285 if(p>=2){
shobonwarrior 0:996353c69d55 286 t2=0;
shobonwarrior 0:996353c69d55 287 RLP1=0;
shobonwarrior 0:996353c69d55 288 }
shobonwarrior 0:996353c69d55 289 }
shobonwarrior 0:996353c69d55 290
shobonwarrior 0:996353c69d55 291 void RL_TWO(void){
shobonwarrior 0:996353c69d55 292 q++;
shobonwarrior 0:996353c69d55 293 if(q>=2){
shobonwarrior 0:996353c69d55 294 t3=0;
shobonwarrior 0:996353c69d55 295 RLP2=t3;
shobonwarrior 0:996353c69d55 296 }
shobonwarrior 0:996353c69d55 297 }
shobonwarrior 0:996353c69d55 298
shobonwarrior 0:996353c69d55 299 void QEI(){
shobonwarrior 0:996353c69d55 300 int a,b/*,A,B*/;
shobonwarrior 0:996353c69d55 301 //double c,R3,R4;
shobonwarrior 0:996353c69d55 302 //R3=wheel1.getAngle();
shobonwarrior 0:996353c69d55 303 //R4=wheel2.getAngle();
shobonwarrior 0:996353c69d55 304 a=wheel1.getPulses();
shobonwarrior 0:996353c69d55 305 b=wheel2.getPulses();
shobonwarrior 0:996353c69d55 306 //pc.printf("a:%d b:%d ",a,b);
shobonwarrior 0:996353c69d55 307 if(a<0){
shobonwarrior 0:996353c69d55 308 a=a*(-1);
shobonwarrior 0:996353c69d55 309 }
shobonwarrior 0:996353c69d55 310 if(b<0){
shobonwarrior 0:996353c69d55 311 b=b*(-1);
shobonwarrior 0:996353c69d55 312 }
shobonwarrior 0:996353c69d55 313 /*
shobonwarrior 0:996353c69d55 314 c=(a+b)/2;
shobonwarrior 0:996353c69d55 315 A=a-(int)c;
shobonwarrior 0:996353c69d55 316 B=b-(int)c;
shobonwarrior 0:996353c69d55 317 */
shobonwarrior 0:996353c69d55 318
shobonwarrior 0:996353c69d55 319 M=4+flag*4;
shobonwarrior 0:996353c69d55 320
shobonwarrior 0:996353c69d55 321 if(k>20){
shobonwarrior 0:996353c69d55 322 if(M>a){
shobonwarrior 0:996353c69d55 323 t4+=0.02;
shobonwarrior 0:996353c69d55 324 }
shobonwarrior 0:996353c69d55 325 else if(M<a){
shobonwarrior 0:996353c69d55 326 t4-=0.02;
shobonwarrior 0:996353c69d55 327 }
shobonwarrior 0:996353c69d55 328 if(t4>1.0){
shobonwarrior 0:996353c69d55 329 t4=1.0;
shobonwarrior 0:996353c69d55 330 }
shobonwarrior 0:996353c69d55 331 if(t4<0.0){
shobonwarrior 0:996353c69d55 332 t4=0.0;
shobonwarrior 0:996353c69d55 333 }
shobonwarrior 0:996353c69d55 334
shobonwarrior 0:996353c69d55 335 if(M>b){
shobonwarrior 0:996353c69d55 336 t5+=0.02;
shobonwarrior 0:996353c69d55 337 }
shobonwarrior 0:996353c69d55 338 else if(M<b){
shobonwarrior 0:996353c69d55 339 t5-=0.02;
shobonwarrior 0:996353c69d55 340 }
shobonwarrior 0:996353c69d55 341 if(t5>1.0){
shobonwarrior 0:996353c69d55 342 t5=1.0;
shobonwarrior 0:996353c69d55 343 }
shobonwarrior 0:996353c69d55 344 if(t5<0.0){
shobonwarrior 0:996353c69d55 345 t5=0.0;
shobonwarrior 0:996353c69d55 346 }
shobonwarrior 0:996353c69d55 347 //pc.printf("pwm1:%f pwm2:%f ",t4,t5);
shobonwarrior 0:996353c69d55 348 CP1=t4;
shobonwarrior 0:996353c69d55 349 CP2=t5;
shobonwarrior 0:996353c69d55 350 wheel1.reset();
shobonwarrior 0:996353c69d55 351 wheel2.reset();
shobonwarrior 0:996353c69d55 352 k=0;
shobonwarrior 0:996353c69d55 353 }else{
shobonwarrior 0:996353c69d55 354 k++;
shobonwarrior 0:996353c69d55 355 //flag=0;
shobonwarrior 0:996353c69d55 356 }
shobonwarrior 0:996353c69d55 357
shobonwarrior 0:996353c69d55 358 }