kashiko aoyama
r_test1_2017_10_11_Wed_A
Diff: main.cpp
- Revision:
- 0:996353c69d55
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Oct 11 08:14:54 2017 +0000
@@ -0,0 +1,358 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "ps3CTRLmbed.h"
+/*
+上 下
+R1 L1 ０．５
+R2 L2 遅く
+立ち上がりで停止 pwm=0
+〇 4段階 2個 ０にする
+× 停止
+□ 0 1
+△ 1 0
+*/
+void RL_ONE(void);
+void RL_TWO(void);
+
+QEI wheel1(p30, p29, NC, 624);//p24,p19のモーター
+QEI wheel2(p7, p8, NC, 624);//p25,p20のモーター
+Serial pc(USBTX, USBRX); // tx, rx
+
+void QEI(void);
+
+InterruptIn PH1(p12);//L1とR1のアレ
+InterruptIn PH2(p13);//L2とR2のアレ
+
+/*100行目あたり。□かつS1＝０で動く。△かつRS2=0で動く。*/
+DigitalIn RS1(p5);
+DigitalIn RS2(p6);
+
+DigitalOut ST1(p14);
+DigitalOut ST2(p15);
+DigitalOut UD(p16);
+DigitalOut RL1(p17);
+DigitalOut RL2(p18);
+DigitalOut C1(p19);
+DigitalOut C2(p20);
+
+PwmOut UDP(p21);
+PwmOut RLP1(p22);
+PwmOut RLP2(p23);
+PwmOut CP1(p24);
+PwmOut CP2(p25);
+PwmOut STP(p26);
+
+I2C i2c(p9,p10);
+
+int addr=0x00;//アドレスをいじるならここ
+int ack=0;
+double t1=0;
+double t2=0;
+double t3=0;
+double t4=0;
+double t5=0;
+double t6=0;
+int flag=0;
+int p=0;
+int q=0;
+int k=0;
+int CC=0;
+int M=0; //目標パルス値
+int rs1=0;
+int rs2=0;
+
+int main() {
+ char cmd[4];
+ PS3DefaultSet();
+ while(1) {
+ CheckInputData();
+ for(int i=0;i<4;i++){
+ cmd[i]=(char)psdata[i+3];
+ }
+ i2c.write(addr,cmd,4);
+
+ /*上下*/
+ if(BTTRUE(UP)){/*上*/
+ t1+=0.2;
+ if(t1>1.0){
+ t1=1.0;
+ }
+ UD=0;
+ UDP=t1;
+ }else if(BTTRUE(DOWN)){/*下*/
+ t1+=0.2;
+ if(t1>1.0){
+ t1=1.0;
+ }
+ UD=1;
+ UDP=t1;
+ }else{
+ t1-=0.2;
+ if(t1<0){
+ t1=0.0;
+ }
+ UDP=t1;
+ }
+
+ rs1=RS1;
+ rs2=RS2;
+
+ /*□ △*/
+ if(BTTRUE(SQUARE)&&rs1==0){
+ t6+=0.2;
+ if(t6>1.0){
+ t6=1.0;
+ }
+ ST1=0;
+ STP=t6;
+ ST2=1;
+ STP=t6;
+ }else if(BTTRUE(TRIANGLE)&&rs2==0){
+ t6+=0.2;
+ if(t6>1.0){
+ t6=1.0;
+ }
+ ST1=1;
+ STP=t6;
+ ST2=0;
+ STP=t6;
+ }else{
+ t6-=0.2;
+ if(t6<0.0){
+ t6=0.0;
+ }
+ STP=t6;
+ }
+
+ //PH1.rise(&RL_ONE);
+ //PH2.rise(&RL_TWO);
+
+ /*R1 L1*/
+ if(BTTRUE(R1)){/*R1*/
+ PH1.rise(&RL_ONE);
+ if(p<2){
+ t2+=0.1;
+ if(t2>0.5){
+ t2=0.5;
+ }
+ RL1=0;
+ RLP1=t2;
+ }else{
+ t2=0;
+ RLP1=0;
+ }
+ }else if(BTTRUE(L1)){//L1
+ PH1.rise(&RL_ONE);
+ if(p<2){
+ t2+=0.1;
+ if(t2>0.5){
+ t2=0.5;
+ }
+ RL1=1;
+ RLP1=t2;
+ }else{
+ t2=0;
+ RLP1=0;
+ }
+ }else{
+ t2-=0.15;
+ if(t2<0){
+ t2=0;
+ }
+ RLP1=t2;
+ }
+
+ /*R2 L2*/
+ if(BTTRUE(R2)){/*R2*/
+ PH2.rise(&RL_TWO);
+ if(q<2){
+ t3+=0.1;
+ if(t3>0.5){
+ t3=0.5;
+ }
+ RL2=0;
+ RLP2=t3;
+ }else{
+ t3=0;
+ RLP2=0;
+ }
+ }else if(BTTRUE(L2)){/*L2*/
+ PH2.rise(&RL_TWO);
+ if(q<2){
+ t3+=0.1;
+ if(t3>0.5){
+ t3=0.5;
+ }
+ RL2=1;
+ RLP2=t3;
+ }else{
+ t3=0;
+ RLP2=0;
+ }
+ }else{
+ t3-=0.15;
+ if(t3<0){
+ t3=0;
+ }
+ RLP2=t3;
+ }
+
+ if(p>=2 && BTFALSE(R1) && BTFALSE(L1)){
+ p=0;
+ }
+
+ if(q>=2 && BTFALSE(R2) && BTFALSE(L2)){
+ q=0;
+ }
+
+ /*
+ //〇
+ while(BTTRUE(CIRCLE)){
+ if(BTFALSE(CIRCLE)){
+ flag=flag+1;
+ if(flag>4){
+ flag=1;
+ }
+ break;
+ }
+ }
+ */
+
+ //〇
+ if(BTTRUE(CIRCLE)){
+ CC=1;
+ }
+
+ if(BTFALSE(CIRCLE)){
+ if(CC==1){
+ flag=flag+1;
+ if(flag>4){
+ flag=1;
+ }
+ }
+ CC=0;
+ }
+
+ //×
+ if(BTTRUE(CROSS)){
+ CC=0;
+ flag=0;
+ }
+
+ //〇 ×
+ if(flag>0){//CC>0
+ //if(flag==0){
+ t4+=0.1;
+ t5+=0.1;
+ //}
+ if(t4>(flag*0.2)){
+ t4=flag*0.2;
+ }
+ if(t5>(flag*0.2)){
+ t5=flag*0.2;
+ }
+
+ if(t4>0.8){
+ t4=0.8;
+ //flag=1;
+ }
+ if(t5>0.8){
+ t5=0.8;
+ //flag=1;
+ }
+ C1=0;
+ C2=0;
+ CP1=t4;
+ CP2=t5;
+ QEI();
+ }else{
+ t4-=0.15;
+ t5-=0.15;
+ if(t4<0){
+ t4=0;
+ }
+ if(t5<0){
+ t5=0;
+ }
+ CP1=0;
+ CP2=0;
+ }
+ }
+}
+
+void RL_ONE(void){
+ p++;
+ if(p>=2){
+ t2=0;
+ RLP1=0;
+ }
+}
+
+void RL_TWO(void){
+ q++;
+ if(q>=2){
+ t3=0;
+ RLP2=t3;
+ }
+}
+
+void QEI(){
+ int a,b/*,A,B*/;
+ //double c,R3,R4;
+ //R3=wheel1.getAngle();
+ //R4=wheel2.getAngle();
+ a=wheel1.getPulses();
+ b=wheel2.getPulses();
+ //pc.printf("a:%d b:%d ",a,b);
+ if(a<0){
+ a=a*(-1);
+ }
+ if(b<0){
+ b=b*(-1);
+ }
+ /*
+ c=(a+b)/2;
+ A=a-(int)c;
+ B=b-(int)c;
+ */
+
+ M=4+flag*4;
+
+ if(k>20){
+ if(M>a){
+ t4+=0.02;
+ }
+ else if(M<a){
+ t4-=0.02;
+ }
+ if(t4>1.0){
+ t4=1.0;
+ }
+ if(t4<0.0){
+ t4=0.0;
+ }
+
+ if(M>b){
+ t5+=0.02;
+ }
+ else if(M<b){
+ t5-=0.02;
+ }
+ if(t5>1.0){
+ t5=1.0;
+ }
+ if(t5<0.0){
+ t5=0.0;
+ }
+ //pc.printf("pwm1:%f pwm2:%f ",t4,t5);
+ CP1=t4;
+ CP2=t5;
+ wheel1.reset();
+ wheel2.reset();
+ k=0;
+ }else{
+ k++;
+ //flag=0;
+ }
+
+}
\ No newline at end of file