Arduino 101 BLE Rover

1/*
2 * Arduino 101 based 4WD Rover code developed utilizing:  
3 *   - an Arduino 101 microcontroller board - using the Bluetooth LE radio (BLE), and   the 6-axis Inertial Measurement Unit (IMU)
4 * - motors and chassis from Skeleton   Bot - 4WD hercules mobile robotic platform from Seeedstudio - early version 
5   * - Grove I2C Motor Driver Board
6 * - Grove Starter Kit for Arduino from Seeedstudio   - Grove Button, Grove I2C RGB LCD Display, Grove LED Socket, 
7 * - HC-SR04 Ultrasonic   sensor, wired via a small breadboard
8 * 
9 * Most of this project's code is   derived from other Arduino samples found 
10 * on www.instructables.com and www.github.com
11   * 
12 * Much respect and appreciation for the "Makers" who have come before me
13   * Lots of knowledge gained from their code and hope my example here is useful to   others
14 * 
15 * the integration and other bits of original code were written   by:
16 * Author: Dave Shade
17 * 
18 * it is available for use under the GNU   Lesser General Public License
19 * see below for details
20 * 
21 * *** Significant   pieces of code used from the following: ***
22 * 
23 * Code included for the Grove   I2C Motor Driver from:
24 * Grove - I2C motor driver demo v1.0
25 * by: http://www.seeedstudio.com
26   *
27 * Code for the RoverControl function taken from the Basic_Robt.ino Sketch:
28   * http://www.instructables.com/id/Smartphone-Controlled-Arduino-Rover/
29 * by   deba168
30 * 
31 * Code for the Crash Detection derived from the Arduino 101 sample:   ShockDetect
32 *    Copyright (c) 2015 Intel Corporation.  All rights reserved.
33   * Code for the BlueTooth Remote Control derived from the Arduino 101 sample: CallbackLED
34   *    Copyright (c) 2015 Intel Corporation.  All rights reserved.
35 *    
36 *   Specifics for the BLE UART characteristics - to enable the use of the Nordic Semiconductor   UART applicaion
37 * found at: https://www.nordicsemi.com/eng/Products/Nordic-mobile-Apps/nRF-UART-App2
38   * 
39 * Code included from various Grove examples like: 
40 * Grove RGB LCD display,   Grove Button, Grove LED and they are:
41 * 2013 Copyright (c) Seeed Technology   Inc.  All right reserved.
42 * 
43 * All code utilized is covered under the license   agreement described below: 
44 *  
45 *  This demo code is free software; you   can redistribute it and/or
46 *  modify it under the terms of the GNU Lesser General   Public
47 *  License as published by the Free Software Foundation; either
48 *   version 2.1 of the License, or (at your option) any later version.
49 *
50 *   This library is distributed in the hope that it will be useful,
51 *  but WITHOUT   ANY WARRANTY; without even the implied warranty of
52 *  MERCHANTABILITY or FITNESS   FOR A PARTICULAR PURPOSE.  See the GNU
53 *  Lesser General Public License for   more details.
54 *
55 *  You should have received a copy of the GNU Lesser General   Public
56 *  License along with this library; if not, write to the Free Software
57   *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
58   *
59*/
60
61#include <Wire.h>   // Library for the initializing the I2C bus.
62#include   <CurieBLE.h>   // CurieBLE library - pre-installed when Arduino 101 is selected   in Arduino IDE 1.6.7 or later 
63
64//I2C Grove Motor Driver defines
65//Motor   controller defines
66#define MotorSpeedSet             0x82
67#define PWMFrequenceSet            0x84
68#define DirectionSet              0xaa
69#define MotorSetA                 0xa1
70#define   MotorSetB                 0xa5
71#define Nothing                   0x01
72#define   EnableStepper             0x1a
73#define UnenableStepper           0x1b
74#define   Stepernu                  0x1c
75#define I2CMotorDriverAdd         0x0f // Set   the address of the I2CMotorDriver
76
77// Variables for allocating the Digital   I/O pins of the Arduino
78//const int resetPin = 2;           // D2 used by Grove   Button - reset from crash state
79const int ledPin = 3;             // D3 used   by Grove LED socket - showing bluetooth connected
80//const int servoPin = 5;         //   D5 used by the servo
81//const int ultrasonic_back = 6;  // D6 used by the Back   ultrasonic sensor
82                                  // D7 used by the Back ultrsonic   sensor
83//const int ultrasonic_front = 8;   // D8 used by the Front ultrasonic   sensor
84                                  // D9 used by the Front ultrasonic sensor
85//   0x0f is the I2C address used by Grove Motor Driver board
86// 0x3e is an I2C address   used by the Grove RGB LCD display (7c>>1)
87// 0x62 is an I2C address used by the   Grove RGB LCD display (c4>>1)
88
89
90char vSpeedSet = 50;    // this variable   holds the speed setting from the remote control - default speed is half
91char   vSpeedLimit = 50;  // this variable holds the limited value of the speed based on   sensor values
92
93char state = 'c';  // initial state is stop
94char prev_state   = 'c';
95
96BLEPeripheral blePeripheral;  // BLE Peripheral Device (the board   you're programming)
97// ====  create Nordic Semiconductor BLE UART service =========
98//   Must use these UUIDs and BLE characteristics
99BLEService uartService = BLEService("6E400001B5A3F393E0A9E50E24DCCA9E");
100//   create characteristics
101BLECharacteristic rxCharacteristic = BLECharacteristic("6E400002B5A3F393E0A9E50E24DCCA9E",   BLEWriteWithoutResponse, 20);   // == TX on central (android app)
102BLECharacteristic   txCharacteristic = BLECharacteristic("6E400003B5A3F393E0A9E50E24DCCA9E", BLENotify   , 20);   // == RX on central (android app)
103 
104
105// Setup function - run once   at reset or power-on
106void setup()  {
107  Wire.begin();   // join i2c bus (address   optional for master)
108  delay(100);
109  Serial.begin(115200);
110  // initialize   the Reset button on D2 and the Blue Grove LED on D3
111  pinMode(ledPin, OUTPUT);    // use the LED on pin 3 as an output
112
113  // set advertised local name and   service UUID:
114  blePeripheral.setLocalName("4WD_RV");   //make unique name
115   blePeripheral.setAdvertisedServiceUuid(uartService.uuid());
116
117  // add service   and characteristic:
118  blePeripheral.addAttribute(uartService);
119  blePeripheral.addAttribute(rxCharacteristic);
120   blePeripheral.addAttribute(txCharacteristic);
121
122  // assign event handlers   for connected, disconnected to peripheral
123  blePeripheral.setEventHandler(BLEConnected,   blePeripheralConnectHandler);
124  blePeripheral.setEventHandler(BLEDisconnected,   blePeripheralDisconnectHandler);
125
126  // assign event handler for characteristic
127   rxCharacteristic.setEventHandler(BLEWritten, rxCharacteristicWritten);
128
129   // begin advertising BLE service:
130  blePeripheral.begin();
131  //Serial.println(("Bluetooth   device active, waiting for connections..."));  // for debugging
132  
133  // Initialize   the motor controllers  
134  MotorDirectionSet(0b0000);  //0b0000  stopped 
135   delay(100);
136  MotorSpeedSetAB(vSpeedSet,vSpeedSet); // on a scale of 1 to 100
137   delay(100);  
138}  // end setup
139
140void loop()  {
141 /* Key sections of   the loop
142  *  Check if we are in a "crash" condition
143  *  if not crashed:
144   *    poll for a new value written into state by the rxCharacteristic
145  *    check   if there is something new in "state"  
146  *      if yes call rovercontrol
147   *      if no call speedcontrol
148  *  if crashed: 
149  *    look for a reset   button press
150  */
151    blePeripheral.poll();
152    if(state != prev_state)   {   // check if the value from the remote control new or the same as the previous   one?
153      RoverControl(state);   // function to respond to control state changes   - direction and/or speed
154      //Serial.println(char(state));
155      prev_state   = state;
156    }
157}
158
159void blePeripheralConnectHandler(BLECentral& central)   {   // central connected event handler
160  Serial.print("Connected event, central:   ");
161  Serial.println(central.address());
162  digitalWrite(ledPin, HIGH);     
163   //Serial.println("LED on");
164}
165
166void blePeripheralDisconnectHandler(BLECentral&   central) {   // central disconnected event handler
167  Serial.print("Disconnected   event, central: ");
168  Serial.println(central.address());
169  digitalWrite(ledPin,   LOW);      
170  //Serial.println("LED off");
171  state = 'c';
172  RoverControl(state);             // stop rover on BLE disconnect - should reduce runaway rover incidents
173   //Serial.println(char(state));
174  delay(100);
175}
176
177void rxCharacteristicWritten(BLECentral&   central, BLECharacteristic& characteristic) {
178  // central wrote new value to   characteristic, update LED
179  //Serial.print("Characteristic event, written:   ");
180  if (characteristic.value()) {   // NULL pointer check
181    state = *characteristic.value();    // de-reference to get first byte
182    //Serial.println(char(state));
183  }   
184}
185
186// Functions to set the 2 DC motor's speed: motorSpeedA: the DC motor   A speed; should be 0~100, motorSpeedB: the DC motor B speed; should be 0~100;
187void   MotorSpeedSetAB(unsigned char MotorSpeedA , unsigned char MotorSpeedB)  {
188  MotorSpeedA=map(MotorSpeedA,0,100,0,255);
189   MotorSpeedB=map(MotorSpeedB,0,100,0,255);
190  Wire.beginTransmission(I2CMotorDriverAdd);   // transmit to device I2CMotorDriverAdd
191  Wire.write(MotorSpeedSet);   // set   pwm header 
192  Wire.write(MotorSpeedA);     // send pwma 
193  Wire.write(MotorSpeedB);      // send pwmb    
194  Wire.endTransmission();    // stop transmitting
195}
196
197//   set the direction of DC motor. 
198void MotorDirectionSet(unsigned char Direction)   {   //  Adjust the direction of the motors 0b0000 I4 I3 I2 I1
199  Wire.beginTransmission(I2CMotorDriverAdd);    // transmit to device I2CMotorDriverAdd
200  Wire.write(DirectionSet);   // Direction   control header
201  Wire.write(Direction);   // send direction control information
202   Wire.write(Nothing);   // need to send this byte as the third byte(no meaning)   
203  Wire.endTransmission();   // stop transmitting 
204}
205
206void RoverControl(char   state) {
207 /*
208  * Respond to changes in direction - forward, left, stop, right,   backward from Bluetooth LE Remote Control
209  *
210  * With a new "state" from   the remote control - check for action to take
211  * a - set gear to go forward   (drive)
212  * b - set gear to turn left and go forward
213  * c - set gear to stop   (park)
214  * d - set gear to turn right and go forward
215  * e - set gear to go   backward (reverse)
216  * 0 - set speed to 0 - not implemented in the remote control
217   * 1 - set speed to 25%
218  * 2 - set speed to 50%
219  * 3 - set speed to 75%   
220  * 4 - set speed to 100%
221  */
222  if (state == 'a') {
223    MotorDirectionSet(0b1001);   }  //0b1001    // If state is equal with letter 'a', Motors Rotating in the forward   direction, rover will go forward 
224  else if (state == 'b') {
225    MotorDirectionSet(0b0001);   }  //0b0001    // If state is equal with letter 'b', right motors rotating forward,   left motors off, , rover will turn left
226  else if (state == 'd') {
227    MotorDirectionSet(0b1000);   }  //0b1000    // If state is equal with letter 'd', left motors rotating forward,   right motors off, rover will turn right
228  else if (state == 'c'){
229    MotorDirectionSet(0b0000);   }  //0b0000    // If state is equal with letter 'c', Motors off - stop the rover
230   else if (state == 'e') {
231    MotorDirectionSet(0b0110); }  //0b0110    // If   state is equal with letter 'e', rover will go backward, both motors rotating backward
232   else if (state == '0') {   // Change speed if state is equal from 0 to 4. Values   must be from 0 to 100 - this is mapped to 0 to 255 (PWM) by the MotorSpeedSet function
233     vSpeedSet=0; }
234  else if (state == '1') {
235    vSpeedSet=25; }
236  else   if (state == '2') {
237    vSpeedSet=50; }
238  else if (state == '3') {
239    vSpeedSet=75;   }
240  else if (state == '4') {
241    vSpeedSet=100; }
242  delay(10);
243  vSpeedLimit   = vSpeedSet;
244  MotorSpeedSetAB(vSpeedSet,vSpeedSet);   // set motor speed based   on changes on a scale of 1 to 100
245}
246
247
248

1/*
2 * Arduino 101 based 4WD Rover code developed utilizing:  
3 *  - an Arduino 101 microcontroller board - using the Bluetooth LE radio (BLE), and  the 6-axis Inertial Measurement Unit (IMU)
4 * - motors and chassis from Skeleton  Bot - 4WD hercules mobile robotic platform from Seeedstudio - early version 
5  * - Grove I2C Motor Driver Board
6 * - Grove Starter Kit for Arduino from Seeedstudio  - Grove Button, Grove I2C RGB LCD Display, Grove LED Socket, 
7 * - HC-SR04 Ultrasonic  sensor, wired via a small breadboard
8 * 
9 * Most of this project's code is  derived from other Arduino samples found 
10 * on www.instructables.com and www.github.com
11  * 
12 * Much respect and appreciation for the "Makers" who have come before me
13  * Lots of knowledge gained from their code and hope my example here is useful to  others
14 * 
15 * the integration and other bits of original code were written  by:
16 * Author: Dave Shade
17 * 
18 * it is available for use under the GNU  Lesser General Public License
19 * see below for details
20 * 
21 * *** Significant  pieces of code used from the following: ***
22 * 
23 * Code included for the Grove  I2C Motor Driver from:
24 * Grove - I2C motor driver demo v1.0
25 * by: http://www.seeedstudio.com
26  *
27 * Code for the RoverControl function taken from the Basic_Robt.ino Sketch:
28  * http://www.instructables.com/id/Smartphone-Controlled-Arduino-Rover/
29 * by  deba168
30 * 
31 * Code for the Crash Detection derived from the Arduino 101 sample:  ShockDetect
32 *    Copyright (c) 2015 Intel Corporation.  All rights reserved.
33  * Code for the BlueTooth Remote Control derived from the Arduino 101 sample: CallbackLED
34  *    Copyright (c) 2015 Intel Corporation.  All rights reserved.
35 *    
36 *  Specifics for the BLE UART characteristics - to enable the use of the Nordic Semiconductor  UART applicaion
37 * found at: https://www.nordicsemi.com/eng/Products/Nordic-mobile-Apps/nRF-UART-App2
38  * 
39 * Code included from various Grove examples like: 
40 * Grove RGB LCD display,  Grove Button, Grove LED and they are:
41 * 2013 Copyright (c) Seeed Technology  Inc.  All right reserved.
42 * 
43 * All code utilized is covered under the license  agreement described below: 
44 *  
45 *  This demo code is free software; you  can redistribute it and/or
46 *  modify it under the terms of the GNU Lesser General  Public
47 *  License as published by the Free Software Foundation; either
48 *  version 2.1 of the License, or (at your option) any later version.
49 *
50 *  This library is distributed in the hope that it will be useful,
51 *  but WITHOUT  ANY WARRANTY; without even the implied warranty of
52 *  MERCHANTABILITY or FITNESS  FOR A PARTICULAR PURPOSE.  See the GNU
53 *  Lesser General Public License for  more details.
54 *
55 *  You should have received a copy of the GNU Lesser General  Public
56 *  License along with this library; if not, write to the Free Software
57  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
58  *
59*/
60
61#include <Wire.h>   // Library for the initializing the I2C bus.
62#include  "rgb_lcd.h"   // Grove library for the I2C RGB LCD Display is found at: https://github.com/Seeed-Studio/Sketchbook_Starter_Kit_V2.0
63#include  "CurieIMU.h"   // CurieIMU library - pre-installed when Arduino 101 is selected  in Arduino IDE 1.6.7 or later 
64#include <CurieBLE.h>   // CurieBLE library -  pre-installed when Arduino 101 is selected in Arduino IDE 1.6.7 or later 
65
66//I2C  Grove Motor Driver defines
67//Motor controller defines
68#define MotorSpeedSet             0x82
69#define PWMFrequenceSet           0x84
70#define DirectionSet              0xaa
71#define MotorSetA                 0xa1
72#define MotorSetB                 0xa5
73#define Nothing                   0x01
74#define EnableStepper             0x1a
75#define UnenableStepper           0x1b
76#define Stepernu                  0x1c
77#define I2CMotorDriverAdd         0x0f // Set the address  of the I2CMotorDriver
78
79// Variables for allocating the Digital I/O pins of  the Arduino
80const int resetPin = 2;           // D2 used by Grove Button - reset  from crash state
81const int ledPin = 3;             // D3 used by Grove LED socket  - showing bluetooth connected
82//const int servoPin = 5;         // D5 used by  the servo
83//const int ultrasonic_back = 6;  // D6 used by the Back ultrasonic  sensor
84                                  // D7 used by the Back ultrsonic sensor
85//const  int ultrasonic_front = 8;   // D8 used by the Front ultrasonic sensor
86                                  //  D9 used by the Front ultrasonic sensor
87// 0x0f is the I2C address used by Grove  Motor Driver board
88// 0x3e is an I2C address used by the Grove RGB LCD display  (7c>>1)
89// 0x62 is an I2C address used by the Grove RGB LCD display (c4>>1)
90
91//  Variables for the Grove RGB LCD Display - connected to the I2C bus
92// common  RGB values: Red=255,0,0  Green=0,255,0  Blue=0,0,255  Purple=255,0,255  Yellow=255,255,0  Aqua=0,255,255  White=255,255,255 Off=0,0,0
93int colorR = 0;
94int colorG =  255;
95int colorB = 0;
96
97char vSpeedSet = 50;    // this variable holds the  speed setting from the remote control - default speed is half
98char vSpeedLimit  = 50;  // this variable holds the limited value of the speed based on sensor values
99
100bool  crash = false;  // initial crash state is false (operating)
101
102char state =  'c';  // initial state is stop
103char prev_state = 'c';
104
105// Creating the  instance of the LCD display
106rgb_lcd lcd;
107
108BLEPeripheral blePeripheral;  // BLE Peripheral Device (the board you're programming)
109// ====  create Nordic  Semiconductor BLE UART service =========
110// Must use these UUIDs and BLE characteristics
111BLEService  uartService = BLEService("6E400001B5A3F393E0A9E50E24DCCA9E");
112// create characteristics
113BLECharacteristic  rxCharacteristic = BLECharacteristic("6E400002B5A3F393E0A9E50E24DCCA9E", BLEWriteWithoutResponse,  20);   // == TX on central (android app)
114BLECharacteristic txCharacteristic =  BLECharacteristic("6E400003B5A3F393E0A9E50E24DCCA9E", BLENotify , 20);   // ==  RX on central (android app)
115 
116
117// Setup function - run once at reset or  power-on
118void setup()  {
119  Wire.begin();   // join i2c bus (address optional  for master)
120  delay(100);
121  Serial.begin(115200);
122  // initialize the Reset  button on D2 and the Blue Grove LED on D3
123  pinMode(resetPin, INPUT);   //use  the switch which has a pull-up resistor aand connects to ground
124  pinMode(ledPin,  OUTPUT);   // use the LED on pin 3 as an output
125
126  // set advertised local  name and service UUID:
127  blePeripheral.setLocalName("4WD_RV");   //make unique  name
128  blePeripheral.setAdvertisedServiceUuid(uartService.uuid());
129
130  //  add service and characteristic:
131  blePeripheral.addAttribute(uartService);
132  blePeripheral.addAttribute(rxCharacteristic);
133  blePeripheral.addAttribute(txCharacteristic);
134
135  // assign event handlers for connected, disconnected to peripheral
136  blePeripheral.setEventHandler(BLEConnected,  blePeripheralConnectHandler);
137  blePeripheral.setEventHandler(BLEDisconnected,  blePeripheralDisconnectHandler);
138
139  // assign event handler for characteristic
140  rxCharacteristic.setEventHandler(BLEWritten, rxCharacteristicWritten);
141
142  // begin advertising BLE service:
143  blePeripheral.begin();
144  //Serial.println(("Bluetooth  device active, waiting for connections..."));  // for debugging
145  
146 /* Initialise  the IMU and create an Interrupt service routine to deal with a shock - indicating  a crash
147  * Shutting off the electric motors after a big shock can help to minimize  damage to rover motors and circuitry
148  */
149    
150  CurieIMU.begin();
151  CurieIMU.attachInterrupt(CrashEventCallback);   // Enable Shock Detection 
152  /* 
153  * The Threshold value should be adjusted through some experimentation 
154  * so that crashes are detected at reasonable shock levels
155  * this is to ensure  the safety of the operator and to minimize stress on the motors
156  * typical values  for the function are from 1500 - 2000
157  * there were updates to the Arduino 101  firmware and CurieIMU library in July 2016
158  * please ensure you have the latest  version using the board manager of the Arduino IDE
159  * 
160  */
161  CurieIMU.setDetectionThreshold(CURIE_IMU_SHOCK,  1900);   // 2.0g = 2000mg; 1.5g = 1500mg, etc.
162  CurieIMU.setDetectionDuration(CURIE_IMU_SHOCK,  50);   // 50ms
163  CurieIMU.interrupts(CURIE_IMU_SHOCK);
164  //Serial.println("IMU  initialization complete, waiting for events...");   // for debugging
165
166  //Serial.println("LCD  setup begin");    // for debugging
167  // set up the grove RGB LCD's number of  columns and rows:
168  lcd.begin(16, 2);
169  lcd.setRGB(colorR, colorG, colorB);  
170
171  // Print message headers to the LCD.
172  lcd.setCursor(0,0);  // cursor  to home position - it starts there...
173  lcd.print("motor speed % =");
174  //lcd.setCursor(0,  1);
175  //lcd.print("speed  = ");
176
177  // Initialize the motor controllers  
178  MotorDirectionSet(0b0000);  //0b0000  stopped 
179  delay(100);
180  MotorSpeedSetAB(vSpeedSet,vSpeedSet);  // on a scale of 1 to 100
181  delay(100);  
182}  // end setup
183
184void loop()  {
185 /* Key sections of the loop
186  *  Check if we are in a "crash" condition
187  *  if not crashed:
188  *    poll for a new value written into state by the rxCharacteristic
189  *    check if there is something new in "state"  
190  *      if yes call rovercontrol
191  *      if no call speedcontrol
192  *  if crashed: 
193  *    look for a reset  button press
194  */
195  if (crash == false) {   //rover is operating normally
196    blePeripheral.poll();
197    if(state != prev_state) {   // check if the value  from the remote control new or the same as the previous one?
198      RoverControl(state);   // function to respond to control state changes - direction and/or speed
199      //Serial.println(char(state));
200      prev_state = state;
201    }
202  }
203  else {       //rover is in a crashed state
204    if (digitalRead(resetPin) ==  HIGH)  {   // look for reset condition - this occurs from a push button after a  crash
205      CrashRecovery();  // function to recover from crash, start operating  normally again
206    }
207  }
208}
209
210void blePeripheralConnectHandler(BLECentral&  central) {   // central connected event handler
211  Serial.print("Connected event,  central: ");
212  Serial.println(central.address());
213  digitalWrite(ledPin,  HIGH);     
214  //Serial.println("LED on");
215}
216
217void blePeripheralDisconnectHandler(BLECentral&  central) {   // central disconnected event handler
218  Serial.print("Disconnected  event, central: ");
219  Serial.println(central.address());
220  digitalWrite(ledPin,  LOW);      
221  //Serial.println("LED off");
222  state = 'c';
223  RoverControl(state);            // stop rover on BLE disconnect - should reduce runaway rover incidents
224  //Serial.println(char(state));
225  delay(100);
226}
227
228void rxCharacteristicWritten(BLECentral&  central, BLECharacteristic& characteristic) {
229  // central wrote new value to  characteristic, update LED
230  //Serial.print("Characteristic event, written:  ");
231  if (characteristic.value()) {   // NULL pointer check
232    state = *characteristic.value();   // de-reference to get first byte
233    //Serial.println(char(state));
234  }  
235}
236
237// Functions to set the 2 DC motor's speed: motorSpeedA: the DC motor  A speed; should be 0~100, motorSpeedB: the DC motor B speed; should be 0~100;
238void  MotorSpeedSetAB(unsigned char MotorSpeedA , unsigned char MotorSpeedB)  {
239  MotorSpeedA=map(MotorSpeedA,0,100,0,255);
240  MotorSpeedB=map(MotorSpeedB,0,100,0,255);
241  Wire.beginTransmission(I2CMotorDriverAdd);  // transmit to device I2CMotorDriverAdd
242  Wire.write(MotorSpeedSet);   // set  pwm header 
243  Wire.write(MotorSpeedA);     // send pwma 
244  Wire.write(MotorSpeedB);     // send pwmb    
245  Wire.endTransmission();    // stop transmitting
246}
247
248//  set the direction of DC motor. 
249void MotorDirectionSet(unsigned char Direction)  {   //  Adjust the direction of the motors 0b0000 I4 I3 I2 I1
250  Wire.beginTransmission(I2CMotorDriverAdd);   // transmit to device I2CMotorDriverAdd
251  Wire.write(DirectionSet);   // Direction  control header
252  Wire.write(Direction);   // send direction control information
253  Wire.write(Nothing);   // need to send this byte as the third byte(no meaning)  
254  Wire.endTransmission();   // stop transmitting 
255}
256
257void RoverControl(char  state) {
258 /*
259  * Respond to changes in direction - forward, left, stop, right,  backward from Bluetooth LE Remote Control
260  *
261  * With a new "state" from  the remote control - check for action to take
262  * a - set gear to go forward  (drive)
263  * b - set gear to turn left and go forward
264  * c - set gear to stop  (park)
265  * d - set gear to turn right and go forward
266  * e - set gear to go  backward (reverse)
267  * 0 - set speed to 0 - not implemented in the remote control
268  * 1 - set speed to 25%
269  * 2 - set speed to 50%
270  * 3 - set speed to 75%  
271  * 4 - set speed to 100%
272  */
273  if (state == 'a') {
274    MotorDirectionSet(0b1001);  }  //0b1001    // If state is equal with letter 'a', Motors Rotating in the forward  direction, rover will go forward 
275  else if (state == 'b') {
276    MotorDirectionSet(0b0001);  }  //0b0001    // If state is equal with letter 'b', right motors rotating forward,  left motors off, , rover will turn left
277  else if (state == 'd') {
278    MotorDirectionSet(0b1000);  }  //0b1000    // If state is equal with letter 'd', left motors rotating forward,  right motors off, rover will turn right
279  else if (state == 'c'){
280    MotorDirectionSet(0b0000);  }  //0b0000    // If state is equal with letter 'c', Motors off - stop the rover
281  else if (state == 'e') {
282    MotorDirectionSet(0b0110); }  //0b0110    // If  state is equal with letter 'e', rover will go backward, both motors rotating backward
283  else if (state == '0') {   // Change speed if state is equal from 0 to 4. Values  must be from 0 to 100 - this is mapped to 0 to 255 (PWM) by the MotorSpeedSet function
284    vSpeedSet=0; }
285  else if (state == '1') {
286    vSpeedSet=25; }
287  else  if (state == '2') {
288    vSpeedSet=50; }
289  else if (state == '3') {
290    vSpeedSet=75;  }
291  else if (state == '4') {
292    vSpeedSet=100; }
293  delay(10);
294  vSpeedLimit  = vSpeedSet;
295  MotorSpeedSetAB(vSpeedSet,vSpeedSet);   // set motor speed based  on changes on a scale of 1 to 100
296  lcd.setCursor(0, 1);
297  lcd.print("   ");
298  lcd.setCursor(0, 1);
299  lcd.print(vSpeedSet);
300  delay(10);
301}
302
303
304//  function to reset the Rover after a crash - called when the reset push button is  pushed
305void CrashRecovery(void)  {
306  MotorDirectionSet(0b0000);   //0b0000  motor stop
307  vSpeedSet = 50;
308  vSpeedLimit = vSpeedSet;
309  MotorSpeedSetAB(vSpeedSet,vSpeedSet);   // restore motor speed to previous value - on a scale of 1 to 100
310  // restore  the LCD display to its precrash values
311  lcd.setRGB(0,255,0);   // make backlight  green
312  lcd.setCursor (0,0);
313  lcd.clear();
314  lcd.print("motor speed %  =");
315  lcd.setCursor(0, 1);
316  lcd.print(vSpeedSet);
317  delay(10);
318  crash  = false;
319  state = 'c';
320}
321
322// Callback function from the CurieIMU ShockDetect  Sample
323static void CrashEventCallback(void) {
324  if (CurieIMU.getInterruptStatus(CURIE_IMU_SHOCK))  {
325    // Here is where to define what to do in crash situation
326    // should  definitely turn off the motors for safety
327    MotorDirectionSet(0b0000);  //0b0000  stop motors
328    delay(10);
329    MotorSpeedSetAB(50,50);  // set speed to  50 on a scale of 1 to 100
330    lcd.setRGB(255,0,0);   // make backlight red
331    lcd.setCursor(0,0);   // reset cursor to upper left
332    lcd.clear();   //  clear display
333    lcd.println("CRASH!!!!!!");   // write crash message to lcd
334    delay(10);
335    state = 'c';
336    prev_state = 'c'; 
337    crash = true;
338  }
339}
340

1/*
2 * Arduino 101 based 4WD Rover code developed utilizing:  
3 *  - an Arduino 101 microcontroller board - using the Bluetooth LE radio (BLE), and  the 6-axis Inertial Measurement Unit (IMU)
4 * - motors and chassis from Skeleton  Bot - 4WD hercules mobile robotic platform from Seeedstudio - early version 
5  * - Grove I2C Motor Driver Board
6 * - Grove Starter Kit for Arduino from Seeedstudio  - Grove Button, Grove I2C RGB LCD Display, Grove LED Socket, 
7 * - HC-SR04 Ultrasonic  sensor, wired via a small breadboard
8 * 
9 * Most of this project's code is  derived from other Arduino samples found 
10 * on www.instructables.com and www.github.com
11  * 
12 * Much respect and appreciation for the "Makers" who have come before me
13  * Lots of knowledge gained from their code and hope my example here is useful to  others
14 * 
15 * the integration and other bits of original code were written  by:
16 * Author: Dave Shade
17 * 
18 * it is available for use under the GNU  Lesser General Public License
19 * see below for details
20 * 
21 * *** Significant  pieces of code used from the following: ***
22 * 
23 * Code included for the Grove  I2C Motor Driver from:
24 * Grove - I2C motor driver demo v1.0
25 * by: http://www.seeedstudio.com
26  *
27 * Code for the RoverControl function taken from the Basic_Robt.ino Sketch:
28  * http://www.instructables.com/id/Smartphone-Controlled-Arduino-Rover/
29 * by  deba168
30 * 
31 * Code for the Crash Detection derived from the Arduino 101 sample:  ShockDetect
32 *    Copyright (c) 2015 Intel Corporation.  All rights reserved.
33  * Code for the BlueTooth Remote Control derived from the Arduino 101 sample: CallbackLED
34  *    Copyright (c) 2015 Intel Corporation.  All rights reserved.
35 *    
36 *  Specifics for the BLE UART characteristics - to enable the use of the Nordic Semiconductor  UART applicaion
37 * found at: https://www.nordicsemi.com/eng/Products/Nordic-mobile-Apps/nRF-UART-App2
38  * 
39 * Code included from various Grove examples like: 
40 * Grove RGB LCD display,  Grove Button, Grove LED and they are:
41 * 2013 Copyright (c) Seeed Technology  Inc.  All right reserved.
42 * 
43 * All code utilized is covered under the license  agreement described below: 
44 *  
45 *  This demo code is free software; you  can redistribute it and/or
46 *  modify it under the terms of the GNU Lesser General  Public
47 *  License as published by the Free Software Foundation; either
48 *  version 2.1 of the License, or (at your option) any later version.
49 *
50 *  This library is distributed in the hope that it will be useful,
51 *  but WITHOUT  ANY WARRANTY; without even the implied warranty of
52 *  MERCHANTABILITY or FITNESS  FOR A PARTICULAR PURPOSE.  See the GNU
53 *  Lesser General Public License for  more details.
54 *
55 *  You should have received a copy of the GNU Lesser General  Public
56 *  License along with this library; if not, write to the Free Software
57  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
58  *
59*/
60
61#include <Wire.h>   // Library for the initializing the I2C bus.
62#include  <CurieBLE.h>   // CurieBLE library - pre-installed when Arduino 101 is selected  in Arduino IDE 1.6.7 or later 
63
64//I2C Grove Motor Driver defines
65//Motor  controller defines
66#define MotorSpeedSet             0x82
67#define PWMFrequenceSet           0x84
68#define DirectionSet              0xaa
69#define MotorSetA                 0xa1
70#define  MotorSetB                 0xa5
71#define Nothing                   0x01
72#define  EnableStepper             0x1a
73#define UnenableStepper           0x1b
74#define  Stepernu                  0x1c
75#define I2CMotorDriverAdd         0x0f // Set  the address of the I2CMotorDriver
76
77// Variables for allocating the Digital  I/O pins of the Arduino
78//const int resetPin = 2;           // D2 used by Grove  Button - reset from crash state
79const int ledPin = 3;             // D3 used  by Grove LED socket - showing bluetooth connected
80//const int servoPin = 5;         //  D5 used by the servo
81//const int ultrasonic_back = 6;  // D6 used by the Back  ultrasonic sensor
82                                  // D7 used by the Back ultrsonic  sensor
83//const int ultrasonic_front = 8;   // D8 used by the Front ultrasonic  sensor
84                                  // D9 used by the Front ultrasonic sensor
85//  0x0f is the I2C address used by Grove Motor Driver board
86// 0x3e is an I2C address  used by the Grove RGB LCD display (7c>>1)
87// 0x62 is an I2C address used by the  Grove RGB LCD display (c4>>1)
88
89
90char vSpeedSet = 50;    // this variable  holds the speed setting from the remote control - default speed is half
91char  vSpeedLimit = 50;  // this variable holds the limited value of the speed based on  sensor values
92
93char state = 'c';  // initial state is stop
94char prev_state  = 'c';
95
96BLEPeripheral blePeripheral;  // BLE Peripheral Device (the board  you're programming)
97// ====  create Nordic Semiconductor BLE UART service =========
98//  Must use these UUIDs and BLE characteristics
99BLEService uartService = BLEService("6E400001B5A3F393E0A9E50E24DCCA9E");
100//  create characteristics
101BLECharacteristic rxCharacteristic = BLECharacteristic("6E400002B5A3F393E0A9E50E24DCCA9E",  BLEWriteWithoutResponse, 20);   // == TX on central (android app)
102BLECharacteristic  txCharacteristic = BLECharacteristic("6E400003B5A3F393E0A9E50E24DCCA9E", BLENotify  , 20);   // == RX on central (android app)
103 
104
105// Setup function - run once  at reset or power-on
106void setup()  {
107  Wire.begin();   // join i2c bus (address  optional for master)
108  delay(100);
109  Serial.begin(115200);
110  // initialize  the Reset button on D2 and the Blue Grove LED on D3
111  pinMode(ledPin, OUTPUT);   // use the LED on pin 3 as an output
112
113  // set advertised local name and  service UUID:
114  blePeripheral.setLocalName("4WD_RV");   //make unique name
115  blePeripheral.setAdvertisedServiceUuid(uartService.uuid());
116
117  // add service  and characteristic:
118  blePeripheral.addAttribute(uartService);
119  blePeripheral.addAttribute(rxCharacteristic);
120  blePeripheral.addAttribute(txCharacteristic);
121
122  // assign event handlers  for connected, disconnected to peripheral
123  blePeripheral.setEventHandler(BLEConnected,  blePeripheralConnectHandler);
124  blePeripheral.setEventHandler(BLEDisconnected,  blePeripheralDisconnectHandler);
125
126  // assign event handler for characteristic
127  rxCharacteristic.setEventHandler(BLEWritten, rxCharacteristicWritten);
128
129  // begin advertising BLE service:
130  blePeripheral.begin();
131  //Serial.println(("Bluetooth  device active, waiting for connections..."));  // for debugging
132  
133  // Initialize  the motor controllers  
134  MotorDirectionSet(0b0000);  //0b0000  stopped 
135  delay(100);
136  MotorSpeedSetAB(vSpeedSet,vSpeedSet); // on a scale of 1 to 100
137  delay(100);  
138}  // end setup
139
140void loop()  {
141 /* Key sections of  the loop
142  *  Check if we are in a "crash" condition
143  *  if not crashed:
144  *    poll for a new value written into state by the rxCharacteristic
145  *    check  if there is something new in "state"  
146  *      if yes call rovercontrol
147  *      if no call speedcontrol
148  *  if crashed: 
149  *    look for a reset  button press
150  */
151    blePeripheral.poll();
152    if(state != prev_state)  {   // check if the value from the remote control new or the same as the previous  one?
153      RoverControl(state);   // function to respond to control state changes  - direction and/or speed
154      //Serial.println(char(state));
155      prev_state  = state;
156    }
157}
158
159void blePeripheralConnectHandler(BLECentral& central)  {   // central connected event handler
160  Serial.print("Connected event, central:  ");
161  Serial.println(central.address());
162  digitalWrite(ledPin, HIGH);     
163  //Serial.println("LED on");
164}
165
166void blePeripheralDisconnectHandler(BLECentral&  central) {   // central disconnected event handler
167  Serial.print("Disconnected  event, central: ");
168  Serial.println(central.address());
169  digitalWrite(ledPin,  LOW);      
170  //Serial.println("LED off");
171  state = 'c';
172  RoverControl(state);            // stop rover on BLE disconnect - should reduce runaway rover incidents
173  //Serial.println(char(state));
174  delay(100);
175}
176
177void rxCharacteristicWritten(BLECentral&  central, BLECharacteristic& characteristic) {
178  // central wrote new value to  characteristic, update LED
179  //Serial.print("Characteristic event, written:  ");
180  if (characteristic.value()) {   // NULL pointer check
181    state = *characteristic.value();   // de-reference to get first byte
182    //Serial.println(char(state));
183  }  
184}
185
186// Functions to set the 2 DC motor's speed: motorSpeedA: the DC motor  A speed; should be 0~100, motorSpeedB: the DC motor B speed; should be 0~100;
187void  MotorSpeedSetAB(unsigned char MotorSpeedA , unsigned char MotorSpeedB)  {
188  MotorSpeedA=map(MotorSpeedA,0,100,0,255);
189  MotorSpeedB=map(MotorSpeedB,0,100,0,255);
190  Wire.beginTransmission(I2CMotorDriverAdd);  // transmit to device I2CMotorDriverAdd
191  Wire.write(MotorSpeedSet);   // set  pwm header 
192  Wire.write(MotorSpeedA);     // send pwma 
193  Wire.write(MotorSpeedB);     // send pwmb    
194  Wire.endTransmission();    // stop transmitting
195}
196
197//  set the direction of DC motor. 
198void MotorDirectionSet(unsigned char Direction)  {   //  Adjust the direction of the motors 0b0000 I4 I3 I2 I1
199  Wire.beginTransmission(I2CMotorDriverAdd);   // transmit to device I2CMotorDriverAdd
200  Wire.write(DirectionSet);   // Direction  control header
201  Wire.write(Direction);   // send direction control information
202  Wire.write(Nothing);   // need to send this byte as the third byte(no meaning)  
203  Wire.endTransmission();   // stop transmitting 
204}
205
206void RoverControl(char  state) {
207 /*
208  * Respond to changes in direction - forward, left, stop, right,  backward from Bluetooth LE Remote Control
209  *
210  * With a new "state" from  the remote control - check for action to take
211  * a - set gear to go forward  (drive)
212  * b - set gear to turn left and go forward
213  * c - set gear to stop  (park)
214  * d - set gear to turn right and go forward
215  * e - set gear to go  backward (reverse)
216  * 0 - set speed to 0 - not implemented in the remote control
217  * 1 - set speed to 25%
218  * 2 - set speed to 50%
219  * 3 - set speed to 75%  
220  * 4 - set speed to 100%
221  */
222  if (state == 'a') {
223    MotorDirectionSet(0b1001);  }  //0b1001    // If state is equal with letter 'a', Motors Rotating in the forward  direction, rover will go forward 
224  else if (state == 'b') {
225    MotorDirectionSet(0b0001);  }  //0b0001    // If state is equal with letter 'b', right motors rotating forward,  left motors off, , rover will turn left
226  else if (state == 'd') {
227    MotorDirectionSet(0b1000);  }  //0b1000    // If state is equal with letter 'd', left motors rotating forward,  right motors off, rover will turn right
228  else if (state == 'c'){
229    MotorDirectionSet(0b0000);  }  //0b0000    // If state is equal with letter 'c', Motors off - stop the rover
230  else if (state == 'e') {
231    MotorDirectionSet(0b0110); }  //0b0110    // If  state is equal with letter 'e', rover will go backward, both motors rotating backward
232  else if (state == '0') {   // Change speed if state is equal from 0 to 4. Values  must be from 0 to 100 - this is mapped to 0 to 255 (PWM) by the MotorSpeedSet function
233    vSpeedSet=0; }
234  else if (state == '1') {
235    vSpeedSet=25; }
236  else  if (state == '2') {
237    vSpeedSet=50; }
238  else if (state == '3') {
239    vSpeedSet=75;  }
240  else if (state == '4') {
241    vSpeedSet=100; }
242  delay(10);
243  vSpeedLimit  = vSpeedSet;
244  MotorSpeedSetAB(vSpeedSet,vSpeedSet);   // set motor speed based  on changes on a scale of 1 to 100
245}
246
247
248