Changeset 22 in pacpussensors for trunk/Wifibot/ps3remote.cpp

Timestamp:

Aug 2, 2013, 4:19:44 PM (11 years ago)

Author:

phudelai

Message:

New control mode added + Security with the controller

File:

• trunk/Wifibot/ps3remote.cpp (modified) (9 diffs)

trunk/Wifibot/ps3remote.cpp

@@ -21 +21 @@
     quit = 0;
     b = NULL;
+    
     shmem = new ShMem("dualshock", sizeof(struct dualshockButtons_s));
+    
+    char buf[4] = {1, 1, 1, 1};
+    
+    timer_ = new QTimer();
+    connect(timer_, SIGNAL(timeout()), this, SLOT(wifibotRemanentSending()));
 }
 
@@ -29 +35 @@
     quit = 0;
     b = NULL;
+    
     shmem = new ShMem("dualshock", sizeof(struct dualshockButtons_s));
+    
     socket_ = socket;
+    
+    char buf[4] = {1, 1, 1, 1};
+    
+    timer_ = new QTimer();
+    connect(timer_, SIGNAL(timeout()), this, SLOT(wifibotRemanentSending()));
 }
 
@@ -36 +49 @@
 PS3remote::~PS3remote()
 {
-
-}
-
-
-void PS3remote::sendCmd(uint16_t Ls, uint16_t Rs, int leftF, int rightF)
+}
+
+
+void PS3remote::setCmd(uint16_t Ls, uint16_t Rs, int leftF, int rightF)
 {
     float right, left;
     static char lastLeft = 1;
     static char lastRight = 1;
-    char buf[5] = {0};
+    
     buf[1] = leftF;
     buf[3] = rightF;
@@ -51 +63 @@
     left = ((float)Ls / 127) * 100;
     right = ((float)Rs / 127) * 100;
+    
     if(left > 100.0)
         left = 100.0;
+    
     if(right > 100.0)
         right = 100.0;
+    
     buf[0] = (char)left + 1;
     buf[2] = (char)right + 1;
+    
     if(buf[0] != lastLeft || buf[2] != lastRight) {
         fprintf(stderr, "left = %c%d    right = %c%d\n", buf[1] == 1 ? '+' : '-', buf[0], buf[3] == 1 ? '+' : '-', buf[2]);
-        socket_->sendDatagrams(QByteArray(buf));
-    }
-    
+        sendCmd();
+    }    
 
     lastLeft = buf[0];
@@ -68 +83 @@
 
 
-void PS3remote::mode2()
+void PS3remote::sendCmd()
+{
+    if (b->timeout || !b->available)
+      memset(buf, 1, sizeof(buf)); // Mise a 1 du buffer pour arreter le wifibot
+    
+    socket_->sendDatagrams(QByteArray(buf, 4));
+    timer_->start(500);
+}
+
+
+void PS3remote::modeChar()
 {
     uint16_t leftPWM = b->buttons.stick.leftStick_y;
@@ -74 +99 @@
     int leftF = 1, rightF = 1;
 
-    if(leftPWM < 135 && leftPWM > 120)
+    if(leftPWM < 135 && leftPWM > 120) {
         leftPWM = 0;
-    else {
+    } else {
         if(leftPWM > 128) {
             leftPWM -= 128;
             leftF = -1;
-        }
-        else
+        } else {
             leftPWM = 128 - leftPWM;
-    }
-    if(rightPWM < 135 && rightPWM > 120)
+        }
+    }
+    
+    if(rightPWM < 135 && rightPWM > 120) {
         rightPWM = 0;
-    else {
+    } else {
         if(rightPWM > 128) {
             rightF = -1;
             rightPWM -= 128;
-        }
-        else
+        } else {
             rightPWM = 128 - rightPWM;
-    }
+        }
+    }
+    
     if(leftPWM > 127)
         leftPWM = 126;
+    
     if(rightPWM > 127)
         rightPWM = 126;
-    sendCmd(leftPWM, rightPWM, leftF, rightF);
-}
-
-
-void PS3remote::mode1()
+    
+    setCmd(leftPWM, rightPWM, leftF, rightF);
+}
+
+
+void PS3remote::modePS3()
+{
+    int16_t speed = b->buttons.analog.r2 - b->buttons.analog.l2;
+    float x = (float)b->buttons.stick.leftStick_x;
+    int leftF = 1, rightF = 1;
+        
+   if(speed < 10 && speed > -10) {
+        if(x > 140) {
+            x = x - 128;
+            rightF = -(leftF = 1);
+        } else if (x < 115) {
+            x = 128 - x;
+            leftF = -(rightF = 1);
+        } else {
+            x = 0;
+        }
+        
+        setCmd(x, x, leftF, rightF);    
+    } else {
+      
+        if(speed > 0) {
+            rightF = leftF = 1;
+        } else {
+            rightF = leftF = -1;
+        }
+        
+        float right = 1.0 - (x > 140 ? 0.0 : ((128.0 - x) / 128.0));
+        float left = 1.0 - (x < 115 ? 0.0 : ((x - 128.0) / 128.0));
+
+        speed /= 2.5;
+        speed = abs(speed);
+        
+        setCmd(right * speed, left * speed, leftF, rightF);
+    }    
+}
+
+
+void PS3remote::modeJoystick()
 {
     uint16_t speed = b->buttons.stick.leftStick_y;
     float x = (float)b->buttons.stick.rightStick_x;
     int leftF = 1, rightF = 1;
+    
     if(speed < 140 && speed > 115) {
         if(x > 140) {
             x = x - 128;
             rightF = -(leftF = 1);
-        }
-        else if (x < 115) {
+        } else if (x < 115) {
             x = 128 - x;
             leftF = -(rightF = 1);
-        }
-        else
+        } else {
             x = 0;
-        sendCmd(x, x, leftF, rightF);
-    }
-    else {
+        }
+        
+        setCmd(x, x, leftF, rightF);
+    } else {
         if(speed > 128) {
             rightF = leftF = -1;
             speed -= 128;
-        }
-        else
+        } else {
             speed = 128 - speed;
+        }
+        
         float right = 1.0 - (x > 140 ? 0.0 : ((128.0 - x) / 128.0));
         float left = 1.0 - (x < 115 ? 0.0 : ((x - 128.0) / 128.0));
-        sendCmd(right * speed, left * speed, leftF, rightF);
+        
+        setCmd(right * speed, left * speed, leftF, rightF);
     }
 }
@@ -137 +205 @@
 {
     static int lastState = 0;
-    static int mode = 1;
+    static int mode = 0;
 
     if(b->buttons.digital.select) {
         if(!lastState) {
-            mode = mode == 1 ? 2 : 1;
+            //mode = mode == 1 ? 2 : 1;
             // TODO later
             //blueshock_setLeds(0, mode);
-            lastState = 1;
+            mode = (mode + 1) % 3;
+            lastState = 1;
         }
-    }
-    else
+    } else {
         lastState = 0;
-    
-    if (mode == 1)
-        mode1();
-    else
-        mode2();
-}
-
+    }
+    
+    switch (mode){
+      case 0:
+        modePS3();
+        break;
+      
+      case 1:
+        modeJoystick();
+        break;
+        
+      default:
+      case 2:
+        modeChar();
+        break;
+    }
+}
+
+
+void PS3remote::wifibotRemanentSending()
+{
+    sendCmd();
+    fprintf(stderr, "Remanence\n");
+    timer_->start(500);
+}
 
 void PS3remote::run()
 {
     while(!quit) {
+      
         shmem->wait();
         b = (struct dualshockButtons_s *)shmem->read();
@@ -166 +253 @@
         
         if(!b->available)
-            std::cerr << "Waiting for remote...\n";
+            fprintf(stderr, "Waiting for remote...\n");
     }
 }
@@ -173 +260 @@
 void PS3remote::stop()
 {
+    timer_->stop();
     quit = 1;
 }