Diff: main.cpp

Revision:

3:7c90c4811843

Parent:

2:9189fa810ef8

--- a/main.cpp	Sat Feb 21 07:55:05 2015 +0000
+++ b/main.cpp	Sat Mar 14 11:02:38 2015 +0000
@@ -87,21 +87,32 @@
         tb6612_pwma = 1;
         tb6612_ain1 = 0;
         tb6612_ain2 = 0;
-//        // break
-//        tb6612_pwma = 1;
-//        tb6612_ain1 = 1;
-//        tb6612_ain2 = 1;
     }
 }
 
+void motor_break()
+{
+    // break
+    tb6612_pwma = 1;
+    tb6612_ain1 = 1;
+    tb6612_ain2 = 1;
+}
+
 // you must adjust center position
-#define RL_ADJ (-120)
+#define RL_ADJ      (-120)
+#define RL_CENTER   (1500)
+#define RL_RANGE    (200.0)
 
 // RC servo
 // rl : -1.0 ? 1.0
 void servo (float rl)
 {
-    servo_pwm.pulsewidth_us(1500 + RL_ADJ - (int)(500.0 * rl));
+    servo_pwm.pulsewidth_us(RL_CENTER + RL_ADJ - (int)(RL_RANGE * rl));
+}
+
+void connectionCallback(Gap::Handle_t handle, Gap::addr_type_t peerAddrType, const Gap::address_t peerAddr, const Gap::ConnectionParams_t *params)
+{
+    servo(0);
 }
 
 void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
@@ -109,6 +120,8 @@
     //pc.printf("Disconnected \r\n");
     //pc.printf("Restart advertising \r\n");
     
+    servo_pwm.pulsewidth_us(0);
+    
     ble.startAdvertising();
 }
 
@@ -123,8 +136,9 @@
         //pc.printf("DATA:%02X %02X\n\r",stickData.bytes[0],stickData.bytes[1]);
         
         float m = (float)stickData.value.fb / 128.0;
-        motor(m);
-//        if(!StopFlag) motor(m);
+        if(!StopFlag) motor(m);
+        if( StopFlag && (m == 0)) StopFlag = false;
+//        motor(m);
         float s = (float)stickData.value.lr / 128.0;
         servo(s);
     }
@@ -137,14 +151,14 @@
 // Vout[V] = 2.369V - 0.0369*d[cm]
 // sample = Vout[V]/3.3V * 0xFFFF
 // sample = 47190 - 733.66*d[cm]
-// 10cm : 39853
-// 20cm : 32516
-// 30cm : 25180
-// 40cm : 17843
-// 50cm : 10507
-// 60cm : 3170
+// 10cm : 39853 / 64 = 623
+// 20cm : 32516 / 64 = 508
+// 30cm : 25180 / 64 = 393
+// 40cm : 17843 / 64 = 278
+// 50cm : 10507 / 64 = 164
+// 60cm : 3170 / 64 = 50
 // 64cm : 0
-#define DISTANCE_STOP 17843
+#define DISTANCE_STOP 278
 
 void TimerHandler(void)
 {
@@ -155,12 +169,12 @@
     static uint32_t r_acc = 0;
     static uint32_t l_acc = 0;
     
-    uint16_t r = sensor_r;
+    uint16_t r = sensor_r.read_u16();
     r_acc -= (uint32_t)r_buff[cnt];
     r_acc += (uint32_t)r;
     r_buff[cnt] = r;
     
-    uint16_t l = sensor_l;
+    uint16_t l = sensor_l.read_u16();
     l_acc -= (uint32_t)l_buff[cnt];
     l_acc += (uint32_t)l;
     l_buff[cnt] = l;
@@ -173,10 +187,11 @@
         // Stop! Obstacle Ahead
         if( (r_ave >= DISTANCE_STOP) && (l_ave >= DISTANCE_STOP) )
         {
-            motor(0);
+            //motor(0);
+            motor_break();
             StopFlag = true;
         }else{
-            StopFlag = false;
+            //StopFlag = false;
         }
     }
 }
@@ -185,7 +200,8 @@
 {
     // initialize servo & motor
     servo_pwm.period_ms(20);
-    servo(0);
+    servo_pwm.pulsewidth_us(0);
+//    servo(0);
     motor(0);
     
     Ticker ticker;
@@ -193,6 +209,8 @@
     
     // initialize BLE
     ble.init();
+    
+    ble.onConnection(connectionCallback);
     ble.onDisconnection(disconnectionCallback);
     ble.onDataWritten(WrittenHandler);  
     
@@ -204,8 +222,8 @@
     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
                                     (const uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME) - 1);
-//  ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
-    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
+  ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
+//    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
                                     (const uint8_t *)UUID_BLUEPROPO, sizeof(UUID_BLUEPROPO));
     ble.setAdvertisingInterval(160); // 100ms; in multiples of 0.625ms. 
     ble.addService(serviceBluePropo);