Index: trunk/Wifibot/ps3remote.cpp
===================================================================
--- trunk/Wifibot/ps3remote.cpp	(revision 21)
+++ trunk/Wifibot/ps3remote.cpp	(revision 22)
@@ -21,5 +21,11 @@
     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,6 +35,13 @@
     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,14 +49,13 @@
 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,15 +63,18 @@
     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,5 +83,15 @@
 
 
-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,60 +99,103 @@
     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,27 +205,46 @@
 {
     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,5 +253,5 @@
 	
 	if(!b->available)
-	    std::cerr << "Waiting for remote...\n";
+	    fprintf(stderr, "Waiting for remote...\n");
     }
 }
@@ -173,4 +260,5 @@
 void PS3remote::stop()
 {
+    timer_->stop();
     quit = 1;
 }
