Fork BlueUSB lib for NHK robot contest 2014 TEU JAPAN
Dependencies: C12832_lcd FatFileSystem TextLCD mbed
Fork of PS3_BlueUSB by
Diff: Ps3USB.cpp
- Revision:
- 2:2c4daaf8b15c
- Parent:
- 1:335bce3448c3
--- a/Ps3USB.cpp Sun Jan 12 07:08:34 2014 +0000 +++ b/Ps3USB.cpp Fri Apr 11 15:56:07 2014 +0000 @@ -60,17 +60,26 @@ u8 ps3_data[48]; Serial sabertooth1(p13,p14); Serial sabertooth2(p28,p27); +Serial sabertoothArm(p9,p10); Serial pro(USBTX, USBRX); //TextLCD lcd(p24,p26,p27,p28,p29,p30);//for orange board C12832_LCD lcd; -PwmOut air1(p21); -PwmOut air2(p22); + PwmOut rgb(p23); AnalogIn pot1(p19); +DigitalOut AIN1(p15); +DigitalOut AIN2(p16); +DigitalOut BIN2(p17); +DigitalOut BIN1(p18); +DigitalOut STBY(p12); +PwmOut PWMA(p21); +PwmOut PWMB(p22); + + int map(int val, int b_min, int b_max, int a_min, int a_max) { return (val - b_max) * (a_min - a_max) / (b_min - b_max) + a_max; @@ -255,6 +264,7 @@ sabertooth1.baud(19200); sabertooth2.baud(19200); + sabertoothArm.baud(19200); u8 P_UP =_ps3report->PressureUp; u8 P_DOWN =_ps3report->PressureDown; @@ -267,15 +277,19 @@ u8 L1=_ps3report->PressureL1; u8 R1=_ps3report->PressureR1; + u8 Cir=_ps3report->PressureCircle; - -// u8 L_S_X=_ps3report->LeftStickX; -// u8 L_S_Y=_ps3report->LeftStickY; + u8 Tri=_ps3report->PressureTriangle; + u8 Cro=_ps3report->PressureCross; + u8 Squ=_ps3report->PressureSquare; + int sb1_1=0; int sb1_2=0; int sb2_1=0; int sb2_2=0; + + int arm_val=0; u8 M_1_CW_BIG = M_1_CW_MAX; u8 M_2_CW_BIG = M_2_CW_MAX; @@ -295,20 +309,11 @@ M_1_CCW_BIG=map(pot_int,0,100,66,127); M_2_CCW_BIG=map(pot_int,0,100,194,255); -// M_1_CW_MAX *= pot; -// M_2_CW_MAX *=pot; -// M_1_CCW_MAX *=pot; -// M_2_CCW_MAX *=pot; + int inPin1=0; + int inPin2=0; + int inSTBY=0; - if(Cir!=0){ - air1.write(1.0f); - air2.write(0.0f); - rgb.write(0.0); - }else{ - air1.write(0.0f); - air2.write(0.0f); - rgb.write(1.0); - } + if(P_UP==0&&P_DOWN==0&&P_LEFT==0&&P_RIGHT==0) { // up_lock=false; @@ -477,20 +482,51 @@ sabertooth2.putc(sb2_1); sabertooth2.putc(sb2_2); } -// if((L_S_X<110 || L_S_X>140 || L_S_Y<120 || L_S_Y>140) && !up_lock && !down_lock && !left_lock && !right_lock && !c_clockwise_lock && !clockwise_lock){ -// left_stick_lock=true; -// -// sb1_1=map(L_S_X+L_S_Y) -// } - + if(Tri!=0){ + arm_val=map(Tri,0,255,M_1_CCW_MIN,M_1_CCW_MAX); + sabertoothArm.putc(arm_val); + } + if(Cro!=0){ + arm_val=map(Cro,0,255,M_1_CW_MIN,M_1_CW_MAX); + sabertoothArm.putc(arm_val); + } + if(Tri==0 && Cro==0){ + arm_val=M_1_STOP; + sabertoothArm.putc(arm_val); + } + if(Cir!=0){ + inSTBY=1; + STBY=inSTBY; + AIN1=1; + AIN2=0; + BIN1=0; + BIN2=0; + PWMA=Cir; + } + if(Squ!=0){ + inSTBY=1; + STBY=inSTBY; + BIN1=1; + BIN2=0; + AIN1=0; + AIN2=0; + PWMB=Squ; + } + if(Cir==0 && Squ==0){ + inSTBY=0; + STBY=inSTBY; + } + if(Cir!=0 && Squ!=0){ + inSTBY=0; + STBY=inSTBY; + } + lcd.locate(1,1); // lcd.printf("1_1=%u 1_2=%u 2_1=%u \n 2_2=%u \n L2=%u R2=%u L1=%u R1=%u",sb1_1,sb1_2,sb2_1,sb2_2,L2,R2,L1,R1); - lcd.printf("p=%f,p_i=%d,Cir=%u",pot,pot_int,Cir); + lcd.printf("speed=%d\n",pot_int); if(pro.writeable()>0) { pro.printf("%u,%u,%u,%u,%u,%u,%u,%u,%d\n",sb1_1,sb1_2,sb2_1,sb2_2,L2,R2,L1,R1,pot_int); } -// sabertooth.putc(map(((ps3report*)(data+1))->LeftStickY, 0, 255, 0, 127)); -// sabertooth.putc(map(((ps3report*)(data+1))->LeftStickY, 0, 255, 193,254)); // if (count == 24) printf("LSX LSY RSX RSY UPA RPA DPA RPA L2 R2 L1 R1 TRI CIR CRO SQU ACX ACY ACZ GYZ \r\n"); // printf("%3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %4d %4d %4d %4d \r\n", // _ps3report->LeftStickX,