Revision 2:2c4daaf8b15c, committed 2014-04-11

Comitter:

ppr2013G2

Date:

Fri Apr 11 15:56:07 2014 +0000

Parent:

1:335bce3448c3

Commit message:

final commit;

Changed in this revision

--- 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,