1  // SPDX-FileCopyrightText: 2016 James DeVito for Adafruit Industries
  2  //
  3  // SPDX-License-Identifier: MIT
  4  //
  5  /*********************************************************************
  6   This is an example for our nRF51822 based Bluefruit LE modules
  7    
  8   Modified to drive a 3-wheeled BLE Robot Rover! by http://james.devi.to
  9  
 10   Pick one up today in the Adafruit shop!
 11  
 12   Adafruit invests time and resources providing this open source code,
 13   please support Adafruit and open-source hardware by purchasing
 14   products from Adafruit!
 15  
 16   MIT license, check LICENSE for more information
 17   All text above, and the splash screen below must be included in
 18   any redistribution
 19  *********************************************************************/
 20  
 21  #include <string.h>
 22  #include <Arduino.h>
 23  #include <SPI.h>
 24  #if not defined (_VARIANT_ARDUINO_DUE_X_)
 25    #include <SoftwareSerial.h>
 26  #endif
 27  
 28  #include "Adafruit_BLE.h"
 29  #include "Adafruit_BluefruitLE_SPI.h"
 30  #include "Adafruit_BluefruitLE_UART.h"
 31  
 32  #include "BluefruitConfig.h"
 33  
 34  #include <Wire.h>
 35  #include <Adafruit_MotorShield.h>
 36  // #include "utility/Adafruit_PWMServoDriver.h"
 37  // #include <Servo.h> 
 38  
 39  // Create the motor shield object with the default I2C address
 40  Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
 41  
 42  // And connect 2 DC motors to port M3 & M4 !
 43  Adafruit_DCMotor *L_MOTOR = AFMS.getMotor(4);
 44  Adafruit_DCMotor *R_MOTOR = AFMS.getMotor(3);
 45  
 46  //not used, testing acceleration
 47  // int accelTime = 200;
 48  
 49  //Name your RC here
 50  String BROADCAST_NAME = "adafruit red robot rover";
 51  
 52  String BROADCAST_CMD = String("AT+GAPDEVNAME=" + BROADCAST_NAME);
 53  
 54  Adafruit_BluefruitLE_SPI ble(BLUEFRUIT_SPI_CS, BLUEFRUIT_SPI_IRQ, BLUEFRUIT_SPI_RST);
 55  
 56  
 57  // A small helper
 58  void error(const __FlashStringHelper*err) {
 59    Serial.println(err);
 60    while (1);
 61  }
 62  
 63  // function prototypes over in packetparser.cpp
 64  uint8_t readPacket(Adafruit_BLE *ble, uint16_t timeout);
 65  float parsefloat(uint8_t *buffer);
 66  void printHex(const uint8_t * data, const uint32_t numBytes);
 67  
 68  // the packet buffer
 69  extern uint8_t packetbuffer[];
 70  
 71  char buf[60];
 72  
 73  /**************************************************************************/
 74  /*!
 75      @brief  Sets up the HW an the BLE module (this function is called
 76              automatically on startup)
 77  */
 78  /**************************************************************************/
 79  void setup(void)
 80  {
 81    Serial.begin(9600);
 82  
 83    AFMS.begin();  // create with the default frequency 1.6KHz
 84  
 85    // turn on motors
 86    L_MOTOR->setSpeed(0);
 87    L_MOTOR->run(RELEASE);
 88  
 89    R_MOTOR->setSpeed(0);
 90    R_MOTOR->run(RELEASE);
 91      
 92    Serial.begin(115200);
 93    Serial.println(F("Adafruit Bluefruit Robot Controller Example"));
 94    Serial.println(F("-----------------------------------------"));
 95  
 96    /* Initialize the module */
 97    BLEsetup();
 98    
 99  
100  }
101  
102  int velocity = 0;
103  
104  float x, y;
105  
106  int L_restrict = 0;
107  int R_restrict = 0;
108  
109  unsigned long lastAccelPacket = 0;
110  
111  bool modeToggle = false;
112  
113  void loop(void)
114  {
115      // read new packet data
116      uint8_t len = readPacket(&ble, BLE_READPACKET_TIMEOUT);
117      // if (len == 0) return;
118  
119    // Read from Accelerometer input
120      if( accelMode() ) {
121        lastAccelPacket = millis();
122        modeToggle = true;
123        return;
124      }
125  
126    // Stop motors if accelerometer data is turned off (100ms timeout)
127      if( millis() - lastAccelPacket > 100 & modeToggle) {
128        L_MOTOR->run(RELEASE);
129        R_MOTOR->run(RELEASE);
130        modeToggle = false;
131        return;
132      }
133  
134      //if no accelerometer, use control pad
135      if( !modeToggle ) buttonMode();
136      
137  }
138  
139  
140  bool accelMode(){
141    if (packetbuffer[1] == 'A') {
142            x = parsefloat( packetbuffer + 2 );
143            y = parsefloat( packetbuffer + 6 );
144  
145          if( x <= -0.55 ){
146            x += 0.55;
147            x *= -100.0;
148            L_MOTOR->run( BACKWARD );
149            R_MOTOR->run( BACKWARD );
150            if( x >= 45 ) x = 45;
151            if( x <= 0 ) x = 0;
152            velocity = map( x, 0, 45, 0 ,255 );
153          }
154          else if( x >= -0.25 ){
155            x+= 0.25;
156            x *= 100;
157            L_MOTOR->run( FORWARD );
158            R_MOTOR->run( FORWARD );
159            if( x>= 45 ) x = 45;
160            if( x<= 0 ) x = 0;
161            velocity = map( x, 0, 45, 0, 255 );
162          }
163          else{
164            L_MOTOR->run( RELEASE );
165            R_MOTOR->run( RELEASE );
166            velocity = 0;
167          }
168  
169          //account for L / R accel
170  
171          if( y >= 0.1 ){
172              y -= 0.1;
173              y *= 100;
174              if( y >= 50 ) y = 50;
175              if( y <= 0 ) y = 0;
176  
177              L_restrict = fscale( y, 0.0, 50.0, 0.0, 100.0, -4.0 );
178          }
179          else if( y <= -0.1 ){
180              y += 0.1;
181              y *= -100;
182              if( y>= 50 ) y = 50;
183              if( y<= 0 ) y = 0;
184  
185              R_restrict = fscale( y, 0.0, 50.0, 0.0, 100.0, -4.0 );
186          }
187          else{
188              L_restrict = 0;
189              R_restrict = 0;
190          }
191  
192            float Lpercent = ( 100.0 - L_restrict ) / 100.00 ;
193            float Rpercent = ( 100.0 - R_restrict ) / 100.00 ;
194  
195            // Serial.print( x ); 
196            // Serial.print( "\t" ); 
197            // Serial.print( Lpercent ); 
198            // Serial.print( "\t" ); 
199            // Serial.print( velocity ); 
200            // Serial.print( "\t" ); 
201            // Serial.println( Rpercent ); 
202  
203            L_MOTOR->setSpeed( velocity * Lpercent ); 
204            R_MOTOR->setSpeed( velocity * Rpercent ); 
205  
206            return true;
207      }
208      return false;
209  }
210  
211  bool isMoving = false;
212  
213  bool buttonMode(){
214  
215    static unsigned long lastPress = 0;
216    
217  
218   // Buttons
219    if (packetbuffer[1] == 'B') {
220  
221      uint8_t buttnum = packetbuffer[2] - '0';
222      boolean pressed = packetbuffer[3] - '0';
223  
224      // Serial.println(buttnum);
225  
226    Serial.println(isMoving);
227      if (pressed) {
228        isMoving = true;
229        if(buttnum == 5){
230          L_MOTOR->run(FORWARD);
231          R_MOTOR->run(FORWARD);
232        }
233        if(buttnum == 6){
234          L_MOTOR->run(BACKWARD);
235          R_MOTOR->run(BACKWARD);        
236        }
237        if(buttnum == 7){
238          L_MOTOR->run(RELEASE);
239          R_MOTOR->run(FORWARD);
240        }
241        if(buttnum == 8){
242          L_MOTOR->run(FORWARD);
243          R_MOTOR->run(RELEASE);        
244        }
245  
246        lastPress = millis();
247        
248          L_MOTOR->setSpeed(255); 
249          R_MOTOR->setSpeed(255);  
250      } 
251  
252      else {
253        isMoving = false;
254        L_MOTOR->run(RELEASE);
255        R_MOTOR->run(RELEASE);
256      }
257       return true; 
258    }
259      // if(isMoving){
260        
261        // unsigned long timeSincePress = millis() - lastPress;
262  
263        // if(timeSincePress <= accelTime){
264  
265        //   Serial.println( timeSincePress ) ;
266          
267        //   int motorSpeed = map( timeSincePress, 0, accelTime, 0, 255 );
268          
269        //   L_MOTOR->setSpeed(motorSpeed); 
270        //   R_MOTOR->setSpeed(motorSpeed); 
271        // }
272        
273        // else{
274         // // full speed ahead!
275          // L_MOTOR->setSpeed(255); 
276          // R_MOTOR->setSpeed(255);  
277        // }
278     // }
279  
280    return false;
281  
282  }
283  
284  void BLEsetup(){
285    Serial.print(F("Initialising the Bluefruit LE module: "));
286  
287    if ( !ble.begin(VERBOSE_MODE) )
288    {
289      error(F("Couldn't find Bluefruit, make sure it's in CoMmanD mode & check wiring?"));
290    }
291    Serial.println( F("OK!") );
292  
293    /* Perform a factory reset to make sure everything is in a known state */
294    Serial.println(F("Performing a factory reset: "));
295    if (! ble.factoryReset() ){
296         error(F("Couldn't factory reset"));
297    }
298  
299    //Convert the name change command to a char array
300    BROADCAST_CMD.toCharArray(buf, 60);
301  
302    //Change the broadcast device name here!
303    if(ble.sendCommandCheckOK(buf)){
304      Serial.println("name changed");
305    }
306    delay(250);
307  
308    //reset to take effect
309    if(ble.sendCommandCheckOK("ATZ")){
310      Serial.println("resetting");
311    }
312    delay(250);
313  
314    //Confirm name change
315    ble.sendCommandCheckOK("AT+GAPDEVNAME");
316  
317    /* Disable command echo from Bluefruit */
318    ble.echo(false);
319  
320    Serial.println("Requesting Bluefruit info:");
321    /* Print Bluefruit information */
322    ble.info();
323  
324    Serial.println(F("Please use Adafruit Bluefruit LE app to connect in Controller mode"));
325    Serial.println(F("Then activate/use the sensors, color picker, game controller, etc!"));
326    Serial.println();
327  
328    ble.verbose(false);  // debug info is a little annoying after this point!
329  
330    /* Wait for connection */
331    while (! ble.isConnected()) {
332        delay(500);
333    }
334  
335    Serial.println(F("*****************"));
336  
337    // Set Bluefruit to DATA mode
338    Serial.println( F("Switching to DATA mode!") );
339    ble.setMode(BLUEFRUIT_MODE_DATA);
340  
341    Serial.println(F("*****************"));
342  }
343  
344  //Logarithmic mapping function from http://playground.arduino.cc/Main/Fscale
345  float fscale( float inputValue,  float originalMin, float originalMax, float newBegin, float newEnd, float curve){
346  
347    float OriginalRange = 0;
348    float NewRange = 0;
349    float zeroRefCurVal = 0;
350    float normalizedCurVal = 0;
351    float rangedValue = 0;
352    boolean invFlag = 0;
353  
354  
355    // condition curve parameter
356    // limit range
357  
358    if (curve > 10) curve = 10;
359    if (curve < -10) curve = -10;
360  
361    curve = (curve * -.1) ; // - invert and scale - this seems more intuitive - postive numbers give more weight to high end on output 
362    curve = pow(10, curve); // convert linear scale into lograthimic exponent for other pow function
363  
364    /*
365     Serial.println(curve * 100, DEC);   // multply by 100 to preserve resolution  
366     Serial.println(); 
367     */
368  
369    // Check for out of range inputValues
370    if (inputValue < originalMin) {
371      inputValue = originalMin;
372    }
373    if (inputValue > originalMax) {
374      inputValue = originalMax;
375    }
376  
377    // Zero Refference the values
378    OriginalRange = originalMax - originalMin;
379  
380    if (newEnd > newBegin){ 
381      NewRange = newEnd - newBegin;
382    }
383    else
384    {
385      NewRange = newBegin - newEnd; 
386      invFlag = 1;
387    }
388  
389    zeroRefCurVal = inputValue - originalMin;
390    normalizedCurVal  =  zeroRefCurVal / OriginalRange;   // normalize to 0 - 1 float
391  
392    /*
393    Serial.print(OriginalRange, DEC);  
394     Serial.print("   ");  
395     Serial.print(NewRange, DEC);  
396     Serial.print("   ");  
397     Serial.println(zeroRefCurVal, DEC);  
398     Serial.println();  
399     */
400  
401    // Check for originalMin > originalMax  - the math for all other cases i.e. negative numbers seems to work out fine 
402    if (originalMin > originalMax ) {
403      return 0;
404    }
405  
406    if (invFlag == 0){
407      rangedValue =  (pow(normalizedCurVal, curve) * NewRange) + newBegin;
408  
409    }
410    else     // invert the ranges
411    {   
412      rangedValue =  newBegin - (pow(normalizedCurVal, curve) * NewRange); 
413    }
414  
415    return rangedValue;
416  }
417