Diff: main.cpp

Revision:

2:b3bd4f835893

Parent:

1:d4624c8ba9f5

--- a/main.cpp	Mon Nov 09 13:48:59 2015 +0000
+++ b/main.cpp	Thu Nov 12 11:10:45 2015 +0000
@@ -3,6 +3,10 @@
 I2C i2cBus(PB_9, PB_8);
 
 Serial pc(SERIAL_TX, SERIAL_RX);
+DigitalOut led(PA_5);
+
+PwmOut      servoMotor(PA_7);
+DigitalOut  direction(PA_6);
 
 #define NUNCHUK_8BIT_ADDR 0x52<<1  //   8-bit I2C slave address  = (  ( 7bit i2c address) <<  1  |  (read/write) bit 
 
@@ -58,13 +62,52 @@
 
 int main()
 {
+    servoMotor.period(.01);
     NunchukData nunchukData;
 
     initNunchuk();
     while(true) {
         GetNunchukData(nunchukData);
         printf("x-yoy %3d,y-yoy %3d ",nunchukData.xStick,nunchukData.yStick );
+        printf("x-acc %3d,y-acc %3d,z-acc %3d ",nunchukData.xAcc,nunchukData.yAcc,nunchukData.zAcc);
+        printf("ButtonZ %d,ButtonC %d, ",nunchukData.bButtonZ,nunchukData.bButtonC);
         printf("\r\n");
+        led=nunchukData.bButtonC;
+
+        if( !nunchukData.bButtonZ) {
+            if(nunchukData.xStick<135) {
+                float speed=(135.0f-(float)nunchukData.xStick)/100.0f;
+                printf("%f",speed);
+                direction=0;
+                servoMotor.write(speed); //
+            } else if(nunchukData.xStick>145) {
+                float speed=((float)nunchukData.xStick-145.0f)/100.0f;
+                printf("%f",speed);
+                direction=1;
+                servoMotor.write(1.0f-speed);
+            } else {
+                direction=1;
+                servoMotor.write(1);
+            }
+        }
+
+
+        if( nunchukData.bButtonZ) {
+            if(nunchukData.xAcc<495) {
+                float speed=(495.0f-(float)nunchukData.xAcc)/100.0f;
+                printf("%f",speed);
+                direction=0;
+                servoMotor.write(speed); //
+            } else if(nunchukData.xAcc>505) {
+                float speed=((float)nunchukData.xAcc-505.0f)/100.0f;
+                printf("%f",speed);
+                direction=1;
+                servoMotor.write(1.0f-speed);
+            } else {
+                direction=1;
+                servoMotor.write(1);
+            }
+        }
     }
 }