ramo
160801
Dependencies: BLE_API mbed nRF51822
Fork of
151006_1st_Scenario_normal
by 
ramo
Diff: main.cpp
- Revision:
- 4:e80d411f87ae
- Parent:
- 3:3f4aa38b0fb7
--- a/main.cpp Thu Oct 01 17:33:32 2015 +0000
+++ b/main.cpp Mon Aug 01 05:20:24 2016 +0000
@@ -13,7 +13,9 @@
* See the License for the specific language governing permissions and
* limitations under the License.
*/
-
+#include <cstdlib>
+#include <ctime>
+#include <math.h>
#include "mbed.h"
#include "ble/BLE.h"
#include "ble/DiscoveredCharacteristic.h"
@@ -29,34 +31,68 @@
DigitalOut pin29(P0_29);
static const unsigned NUM_ROBOTS = 8;
-static const unsigned ADDR_LEN = 6;
+static const unsigned CLASS = 0;
uint16_t customServiceUUID = 0xA000;
uint16_t readwriteCharUUID = 0xA001;
static const uint16_t secure_code[] = {0xABCD};
uint8_t info[NUM_ROBOTS]= {0};
+uint8_t x[22] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22};
+float y[22];
-ReadWriteArrayGattCharacteristic<uint8_t, NUM_ROBOTS> readwriteChar(readwriteCharUUID, info, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE);
-
-//GattCharacteristic readwriteChar (readwriteCharUUID, info, NUM_ROBOTS, NUM_ROBOTS, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE);
+ReadWriteArrayGattCharacteristic<uint8_t, NUM_ROBOTS> readwriteChar(readwriteCharUUID, info);
GattCharacteristic *characteristics[] = {&readwriteChar};
GattService customService(customServiceUUID, characteristics, sizeof(characteristics) / sizeof(GattCharacteristic *));
-bool triggerLedCharacteristic = false;
DiscoveredCharacteristic characteristics_discovered;
-int8_t temp;
-uint8_t pt_addr = 0;
-uint8_t list_addr[NUM_ROBOTS][ADDR_LEN+NUM_ROBOTS]= {0,};
-int8_t list_rssi[NUM_ROBOTS];
-uint8_t initial_list_addr[NUM_ROBOTS][ADDR_LEN + 1]= {0,};
-// 1st Device, 2nd device has '10(2)' information
+uint8_t com_p;
+bool movingTrigger = false;
uint8_t info_t[NUM_ROBOTS]= {0,};
uint8_t point;
+float IRRead(PinName Pin) {
+ AnalogIn AIn(Pin);
+ float IRIn;
+ IRIn=AIn;
+ return(IRIn);
+}
+
+void moveRandom(void)
+{
+ uint8_t move_r1 = static_cast <int> (rand())%2; //0 or 1
+ float move_r2 = 3 * static_cast <float> (rand()) / static_cast <float> (RAND_MAX); // 0~3
+
+ if( move_r1 > 0.5) {
+ move_r1 = 1;
+ } else {
+ move_r1 = 0;
+ }
+
+ if(move_r1==0) {
+ pin4 = 1;
+ pin5 = 1;
+ pin15 = 0;
+ pin29 = 0;
+ } else {
+ pin4 = 0;
+ pin5 = 0;
+ pin15 = 1;
+ pin29 = 1;
+ }
+ wait(move_r2);
+
+ pin4 = 1;
+ pin5 = 1;
+ pin15 = 1;
+ pin29 = 1;
+ float move_r3 = 3 * static_cast <float> (rand()) / static_cast <float> (RAND_MAX); // 0~3
+ wait(move_r3);
+}
+
void RGB_Show(uint8_t r, uint8_t g, uint8_t b)
{
uint8_t rgb[3] = {g, r, b};
@@ -115,121 +151,40 @@
}
}
-void initialSetup(const Gap::AdvertisementCallbackParams_t *params)
+void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params)
{
- if ( params->advertisingData[1] == 0xAF ) {
- for(int i = 0; i < NUM_ROBOTS; i++) {
- if(initial_list_addr[i][5] == params->peerAddr[5]
- && initial_list_addr[i][4] == params->peerAddr[4]
- && initial_list_addr[i][3] == params->peerAddr[3]
- && initial_list_addr[i][2] == params->peerAddr[2]
- && initial_list_addr[i][1] == params->peerAddr[1]
- && initial_list_addr[i][0] == params->peerAddr[0]) {
- break;
- }
- if(initial_list_addr[i][5] == 0 && initial_list_addr[i][4] == 0
- && initial_list_addr[i][3] == 0 && initial_list_addr[i][2] == 0
- && initial_list_addr[i][1] == 0 && initial_list_addr[i][0] == 0) {
- initial_list_addr[i][5] = params->peerAddr[5];
- initial_list_addr[i][4] = params->peerAddr[4];
- initial_list_addr[i][3] = params->peerAddr[3];
- initial_list_addr[i][2] = params->peerAddr[2];
- initial_list_addr[i][1] = params->peerAddr[1];
- initial_list_addr[i][0] = params->peerAddr[0];
-
- initial_list_addr[i][6] = 1;
- break;
- }
- }
- printf("initialsetup\r\n");
- printf("adv peerAddr[%02x %02x %02x %02x %02x %02x] rssi %d, isScanResponse %u, AdvertisementType %u, %0x %0x\r\n",
- params->peerAddr[5], params->peerAddr[4], params->peerAddr[3], params->peerAddr[2], params->peerAddr[1], params->peerAddr[0],
- params->rssi, params->isScanResponse, params->type, params->advertisingData[1], params->advertisingData[0]);
+ float ran=static_cast <float> (rand()) / static_cast <float> (RAND_MAX); // 0~1 probability
+ printf("com_p : %f OOOOOOOOOOOOOO ran : %f \r\n",y[com_p],ran);
+ if( (params->rssi > -48) && (y[com_p] > ran) && (params->advertisingData[1] == 0xAF) ) {
+ ble.gap().connect(params->peerAddr, Gap::ADDR_TYPE_RANDOM_STATIC, NULL, NULL);
+ } else {
+ movingTrigger = true;
}
}
-void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params)
+void characteristicDiscoveryCallback(const DiscoveredCharacteristic *characteristicP)
{
- if ( params->rssi > -50 && params->advertisingData[1] == 0xAF ) {
- printf("adv peerAddr[%02x %02x %02x %02x %02x %02x] rssi %d, isScanResponse %u, AdvertisementType %u, %0x %0x %0x\r\n",
- params->peerAddr[5], params->peerAddr[4], params->peerAddr[3], params->peerAddr[2], params->peerAddr[1], params->peerAddr[0],
- params->rssi, params->isScanResponse, params->type, params->advertisingData[3], params->advertisingData[2], params->advertisingData[1]);
-
- ble.gap().connect(params->peerAddr, Gap::ADDR_TYPE_RANDOM_STATIC, NULL, NULL);
- }
- else{
- printf("adv peerAddr[%02x %02x %02x %02x %02x %02x] rssi %d, isScanResponse %u, AdvertisementType %u, %0x %0x %0x\r\n",
- params->peerAddr[5], params->peerAddr[4], params->peerAddr[3], params->peerAddr[2], params->peerAddr[1], params->peerAddr[0],
- params->rssi, params->isScanResponse, params->type, params->advertisingData[3], params->advertisingData[2], params->advertisingData[1]);
+ if (characteristicP->getUUID() == 0xA001) { /* !ALERT! Alter this filter to suit your device. */
+ characteristics_discovered = *characteristicP;
+ printf("characteristic discovered\r\n");
+ characteristics_discovered.read();
}
- /*
- int i;
-
- for(i = 0; i < NUM_ROBOTS; i++) {
- if(list_addr[i][5] == params->peerAddr[5] && list_addr[i][4] == params->peerAddr[4] && list_addr[i][3] == params->peerAddr[3]
- && list_addr[i][2] == params->peerAddr[2] && list_addr[i][1] == params->peerAddr[1] && list_addr[i][0] == params->peerAddr[0]) {
- list_rssi[i] = params->rssi;
- break;
- }
- if(list_addr[i][5] == 0 && list_addr[i][4] == 0 && list_addr[i][3] == 0 && list_addr[i][2] == 0 && list_addr[i][1] == 0 && list_addr[i][0] == 0) {
- list_addr[i][5] = params->peerAddr[5];
- list_addr[i][4] = params->peerAddr[4];
- list_addr[i][3] = params->peerAddr[3];
- list_addr[i][2] = params->peerAddr[2];
- list_addr[i][1] = params->peerAddr[1];
- list_addr[i][0] = params->peerAddr[0];
-
- list_addr[i][6] = params->advertisingData[0];
-
- list_rssi[i] = params->rssi;
-
- if ( i==NUM_ROBOTS-1 ) {
- for(unsigned j=0; j<NUM_ROBOTS; j++) {
- for(unsigned k=j ; k<NUM_ROBOTS ; k++) {
- if(list_rssi[j] < list_rssi[k]) {
- temp = list_rssi[j];
- list_rssi[j] = list_rssi[k];
- list_rssi[k] = temp;
- for(unsigned l = 0; l < 6 ; l++) {
- temp = list_addr[j][l];
- list_addr[j][l] = list_addr[k][l];
- list_addr[k][l] = temp;
- }
- }
- }
- }
- break;
- }
- }
-
- if( i == NUM_ROBOTS-1 ) {
- if( i == 2 ) {
- ble.gap().stopScan();
- ble.gap().connect(list_addr[0], Gap::ADDR_TYPE_RANDOM_STATIC, NULL, NULL);
- }
- }
- }
- */
}
void serviceDiscoveryCallback(const DiscoveredService *service)
{
}
-void characteristicDiscoveryCallback(const DiscoveredCharacteristic *characteristicP)
-{
- if (characteristicP->getUUID() == 0xA001) { /* !ALERT! Alter this filter to suit your device. */
- characteristics_discovered = *characteristicP;
- printf("characteristic discovered\r\t\n");
- characteristics_discovered.read();
- }
-}
-
void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
{
- printf("connected\r\n");
+ info[7] += 1;
ble.gap().stopAdvertising();
ble.gap().stopScan();
+
+ printf("adv peerAddr[%02x %02x %02x %02x %02x %02x]\r\n",
+ params->peerAddr[5], params->peerAddr[4], params->peerAddr[3], params->peerAddr[2], params->peerAddr[1], params->peerAddr[0]);
+ movingTrigger = false;
+ printf("connected\r\n");
if (params->role == Gap::CENTRAL) {
printf("CENTRAL\r\n");
ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, 0xA000, 0xA001);
@@ -238,27 +193,60 @@
}
}
-void triggerToggledWrite(const GattReadCallbackParams *response)
+void onDataReadCallback(const GattReadCallbackParams *response)
{
if (response->handle == characteristics_discovered.getValueHandle()) {
+ printf("info_t : ");
for(int i=0; i < NUM_ROBOTS; i++) {
- info_t[i] = info[i] || response->data[i];
- info[i] = info_t[i];
+ info_t[i] = response->data[i];
+ printf("%u \t",info_t[i]);
}
- characteristics_discovered.write(NUM_ROBOTS, info);
+ printf("\r\n");
+
+ if(info[0] == 0) {
+ if(response->data[0] == 0) {
+ info[1] += 1; // +1 point
+ info[2] = 0; // connection with normal
+ info_t[1] += 1;
+ info_t[2] = 0;
+ characteristics_discovered.write(NUM_ROBOTS,info_t);
+ } else {
+ info[3] += 1; // -1 point
+ info[2] = 1; // connection with malicious
+ characteristics_discovered.write(NUM_ROBOTS,info_t);
+ }
+ } else {
+ if(response->data[0] == 0) {
+ info_t[3] += 1;
+ info_t[2] = 1;
+ characteristics_discovered.write(NUM_ROBOTS,info_t);
+ printf("aaa\r\n");
+ } else {
+ characteristics_discovered.write(NUM_ROBOTS,info_t);
+ }
+ }
}
}
void onDataWriteCallback(const GattWriteCallbackParams *params)
{
- characteristics_discovered.read();
+ printf("bbb\r\n");
+ ble.updateCharacteristicValue(readwriteChar.getValueHandle(),info,NUM_ROBOTS);
}
void onDataWrittenCallback(const GattWriteCallbackParams *params)
{
- for(int i=0; i<NUM_ROBOTS; i++) {
+ printf("ccc\r\n");
+ for(int i=0; i<NUM_ROBOTS-1; i++) {
info[i] = params->data[i];
}
+ printf("writecallbackparam : ");
+ for(int i=0; i<NUM_ROBOTS; i++) {
+ printf("%u \t",params->data[i]);
+ }
+ printf("\r\n");
+ ble.gattServer().write(readwriteChar.getValueHandle(),info,NUM_ROBOTS);
+ ble.updateCharacteristicValue(readwriteChar.getValueHandle(),params->data,params->len);
printf("data is written well\r\n");
ble.gap().disconnect(params->connHandle,Gap::REMOTE_USER_TERMINATED_CONNECTION);
@@ -266,76 +254,95 @@
void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
{
- printf("disconnected\r\n");
-
+ printf("Current info : ");
for(int i=0; i<NUM_ROBOTS; i++) {
- printf(" %u", info[i]);
+ printf("%u \t",info[i]);
}
printf("\r\n");
-
+ printf("disconnected\r\n");
+ if(info[2] == 0) {
+ if(com_p == 21) {
+ com_p = 21;
+ } else {
+ com_p += 1;
+ }
+ } else if(info[2] == 1) {
+ if(com_p == 0) {
+ com_p = 0;
+ } else {
+ com_p -= 1;
+ }
+ }
ble.gap().startAdvertising();
ble.gap().startScan(advertisementCallback);
}
-
int main(void)
-{
- Ticker ticker;
- ticker.attach(periodicCallback, 0.5);
-
- int num = 0;
-
- ble.init();
- ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_SCANNABLE_UNDIRECTED);
- ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
- ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::RABOT_REDBEAR_BLE_NANO, (uint8_t *)secure_code, sizeof(secure_code));
- ble.setAdvertisingType(GapAdvertisingParams::ADV_SCANNABLE_UNDIRECTED);
- ble.gap().setAdvertisingInterval(32);
- ble.gap().startAdvertising();
+{
+ srand (static_cast <unsigned> (time(0))); //seeding for random
+ for(int i=0; i<22;i++){
+ y[i] = 0.1 + 0.9*(1-exp(-0.2*x[i]));
+ printf("%f ",y[i]);
+ }
+ printf("\r\n");
+ info[0] = CLASS; // normal : 0 & malicious : 1
+ info[1] = 0; // point
+ com_p = 11;
- ble.gap().setScanParams(1014 , 884);
- ble.gap().startScan(initialSetup);
-
- wait(5);
-
- ble.gap().stopAdvertising();
- ble.gap().stopScan();
+ srand (static_cast <unsigned> (time(0)));
- ble.gap().clearAdvertisingPayload();
-
- for(int i = 0; i< NUM_ROBOTS; i++) {
- num += initial_list_addr[i][6];
- }
- info[num] = 1;
-
- printf("initial setting is done\r\n");
-
- printf("initial info : ");
+ Ticker ticker;
+
for(int i=0; i<NUM_ROBOTS; i++) {
printf("%u \t",info[i]);
}
printf("\r\n");
-
- //advertising for real experiment
printf("communication for connection is started\r\n");
-
+ ble.init();
ble.gap().onConnection(connectionCallback);
ble.gap().onDisconnection(disconnectionCallback);
- ble.gattClient().onDataRead(triggerToggledWrite);
+ ble.gattClient().onDataRead(onDataReadCallback);
ble.gattClient().onDataWrite(onDataWriteCallback);
ble.gattServer().onDataWritten(onDataWrittenCallback);
ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::RABOT_REDBEAR_BLE_NANO, (uint8_t *)secure_code, sizeof(secure_code));
- ble.gap().setAdvertisingInterval(29);
+ ble.gap().setAdvertisingInterval(59);
ble.addService(customService);
+
ble.gap().startAdvertising();
- ble.gap().setScanParams(190 /* scan interval */, 181 /* scan window */);
+ ble.gap().setScanParams(125 /* scan interval */, 79 /* scan window */);
ble.gap().startScan(advertisementCallback);
while (true) {
+ /*
+ if(connectionState == false) {
+ ble.gap().startAdvertising();
+ ble.gap().startScan(advertisementCallback);
+ } else {
+ ble.gap().stopAdvertising();
+ ble.gap().stopScan();
+ }
+ */
ble.waitForEvent();
+ if(movingTrigger == true) {
+ moveRandom();
+ } else {
+ pin4 = 0;
+ pin5 = 0;
+ pin15 = 0;
+ pin29 = 0;
+ }
+ if(info[1] == 20) {
+ printf("Game over");
+ break;
+ }
}
+ ble.shutdown();
+ pin4 = 0;
+ pin5 = 0;
+ pin15 = 0;
+ pin29 = 0;
}
\ No newline at end of file