Diff: main.cpp

Revision:

12:0777ec4114c8

Parent:

11:6916c05fde52

--- a/main.cpp	Fri Sep 07 20:17:07 2018 +0000
+++ b/main.cpp	Mon Oct 29 19:05:11 2018 +0000
@@ -1,4 +1,4 @@
-/* mbed Microcontroller Library
+/* mbed MicrocontrollerLL Library
  * Copyright (c) 2006-2013 ARM Limited
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -13,6 +13,7 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
+
  //////////////////////////////////////////////////////////
 ///////////////////////NOTE ///////////////////////////////
 // This program uses BBC microbit as a NRF51822 board    //
@@ -20,9 +21,16 @@
 // BLE-uart/ADC/DIO/PWM capability, see pin maping below //
 ///////////////////////////////////////////////////////////
 ///////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////
+//////////////////////BLE WATER BOT/////////////////////////
+////////////////////////////////////////////////////////////
+
+
         
 #include <string.h>
 #include "mbed.h"
+#include "PID.h"
 #include "BLE.h"
 #include "stdio.h"
 #include "UARTService.h"
@@ -34,6 +42,13 @@
 #include "ConfigParamsPersistence.h"
 
 
+#include "math.h"
+#include "MAG3110.h"
+
+
+ 
+#define INTERVAL 0.1
+
 #define NEED_CONSOLE_OUTPUT 1 // if BLE printf messages needed on the remote console (PC/phone/host);                            
 #if NEED_CONSOLE_OUTPUT
 #define blePrintf(STR) { if (uart) uart->write(STR, strlen(STR)); }
@@ -43,38 +58,71 @@
 
 ///////////////// pin table /////////////////////////
 /////////////////////////////////////////////////////
-// ubit.pin    nrf51822pin    functions      note  //
+// edge.pin    nrf51822pin    functions      note  //
 /////////////////////////////////////////////////////
 //  P2           P0_1        ADC/PWM/DIO      2    //
 //  P1           P0_2        ADC/PWM/DIO      1    // 
 //  P0           P0_3        ADC/PWM/DIO      0    //
+//  P4           P0_5            ADC        COL 2  //
+//  P10          P0_6            ADC        COL 3  //
 //  P16          P_16            DIO               //
 //  P14          P0_25       SPI MIS/DIO           //
 //  P5           P_17        Button A/DI    pullup //
 //  P11          P_26        Button B/DI    pullup //
-
-/////////////////////////////////////////////////////
-///////////////// not mapped yet ////////////////////
-//  P20                      I2C SDA/DIO    pullup //
-//  P19                      I2C SCL/DIO    pullup //
+//  P20          P0_30       I2C SDA/DIO    pullup //
+//  P19          P0_0        I2C SCL/DIO    pullup //
 //  P15                      SPI MOS/DIO           //
 //  P13                      SPI SCK/DIO           //
-//  P16                                            //
-//  P8                                             //
 
 /////////////////////////////////////////////////////
-//      LED Matrix pins are not mapped             //
+//                  LED Matrix pins                //
+//  ROW 1  P0_13   ROW 2  P0_14   ROW 3  P0_15     //
+//  COL 1  P0_4    COL 2  P0_5    COL 3  P0_6      //
+//  COL 4  P0_7    COL 5  P0_8    COL 6  P0_9      //
+//  COL 7  P0_10   COL 8  P0_11   COL 9  P0_12     //
 /////////////////////////////////////////////////////
+/////////// See uBit to nrf51822 Schematic for detailed pinout //////////////////////
+// https://github.com/bbcmicrobit/hardware/blob/master/SCH_BBC-Microbit_V1.3B.pdf  //
+/////////////////////////////////////////////////////////////////////////////////////
+  //Kc, Ti, Td, interval
+          PID controllerL(1.0, 0.1, 0.1, INTERVAL);
+          PID controllerR(1.0, 0.1, 0.1, INTERVAL);
+          
+
 
 
+// Fan drive pwm
+
+PwmOut   LP(P0_2); // left propeller
+PwmOut   RP(P0_1); // right propeller
+
+MAG3110 mag(P0_30, P0_0); // set i2c pins SDA, SCL here //
 BLEDevice  ble;
-DigitalOut led1(LED1);
 UARTService *uart;
-AnalogIn   ain(P0_3);
+ 
+AnalogIn   temperature(P0_3); 
+AnalogIn   turbidity(P0_5);
+AnalogIn   conductivity(P0_6);
+
+DigitalOut   row1(P0_13);  // uBit 1st LED cathode 
+DigitalOut   col1(P0_4); // uBit 1st LED anode
+DigitalIn    swA(P0_17); // uBit button A
+//  col2(P0_5); // adc pin now led removed
+//  col3(P0_6); // adc pin now led removed
+
+//DigitalOut   col1(P0_4);
+
+
 int val,dVal, dec, i;
+int blepush = 0; 
+int refupdate = 0;
 char result[100];
-float batt = 0.03;
- 
+float TEMP = 0.0;
+float TURB = 0.0;
+float COND = 0.0;
+
+float heading = 0.0; // magnetometer heading
+float heading_ref = 0.0;
 
 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
 {
@@ -83,18 +131,60 @@
 
 void periodicCallback(void)
 {
+    blepush = 1;
+   // blePrintf("heading ");
+   // blePrintf(result);
+   // blePrintf("\r\n");
+}
+
+void navigationAdjust (void)
+{
     
-    blePrintf("BATT VOlT: ");
-    blePrintf(result);
-    blePrintf("\r\n");
 }
 
+void calXY() //magnetometer calibration: finding max and min of X, Y axis
+{
+    int tempXmax, tempXmin, tempYmax, tempYmin, newX, newY;
+
+    wait(3);
+
+    tempXmax = tempXmin = mag.readVal(MAG_OUT_X_MSB);
+    tempYmax = tempYmin = mag.readVal(MAG_OUT_Y_MSB);
+    int sample = 0;
+     
+    while(sample<501) {
+        wait(0.05);
+        newX = mag.readVal(MAG_OUT_X_MSB);
+        newY = mag.readVal(MAG_OUT_Y_MSB);
+        if (newX > tempXmax) tempXmax = newX;
+        if (newX < tempXmin) tempXmin = newX;
+        if (newY > tempYmax) tempYmax = newY;
+        if (newY < tempYmin) tempYmin = newY;
+        sample++;
+    col1.write(!(col1.read()));
+    //col3.write(!(col3.read()));
+    
+    }
+   
+    mag.setCalibration( tempXmin, tempXmax, tempYmin, tempYmax );
+ 
+
+}
+
+
 int main(void)
 {
-    Ticker ticker;
-    ticker.attach(periodicCallback, 5);
+    //  pwm freq
+    LP.period(0.01f); 
+    RP.period(0.01f);
+    LP.write(1.0);
+    RP.write(1.0);
+    wait(1);
+    LP.write(0.0);
+    RP.write(0.0);
 
-    blePrintf("Initialising the nRF51822\n\r");
+   row1.write(1);
+ 
     ble.init();
     ble.onDisconnection(disconnectionCallback);
     
@@ -111,28 +201,196 @@
 
     ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
     ble.startAdvertising();
+    
+
+    // may necessary //
+    while(swA.read())
+    {
+    ble.waitForEvent();
+    col1.write(!(col1.read()));
+    wait(1);
+    blePrintf("Hold A to start\n");
+    }
+  // max 20 Character at a time over BLE
+  // max 20 Character at a time over BLE
+  // max 20 Character at a time over BLE
+  
+    
+   ble.waitForEvent();   
+   blePrintf("Init  nRF51822  \n");
+   blePrintf("Calibrating Sensors\n");
+   blePrintf("Rotate To Calibrate\n");
+   blePrintf("when LED blinks fast\n");
+  
+    calXY(); // do the mag calib
+    ble.waitForEvent();
+    
+   blePrintf("Calibrate Completed\n");
+   blePrintf("Put Robot on Water\n");
+  
+    while(swA.read())
+    {
+        col1.write(!(col1.read()));
+        wait(1);
+        blePrintf("Hold A to proceed\n");
+  
+    }
+    
+    Ticker ticker;
+    ticker.attach(&periodicCallback, 5);
+
+    Ticker ticker2;
+    ticker2.attach(&navigationAdjust, .5);
+    
+ 
+  //Reference angle input from 0.0 to 3.3V
+  controllerL.setInputLimits(0.0, 360.0);
+  controllerR.setInputLimits(0.0, 360.0);
+ 
+  //Pwm output from 0.0 to 1.0
+  controllerL.setOutputLimits(0.0, 1.0);
+  controllerR.setOutputLimits(0.0, 1.0);
+  
+  //If there's a bias.
+  controllerL.setBias(0.0);
+  controllerR.setBias(0.0);
+  
+  controllerL.setMode(AUTO_MODE);
+  controllerR.setMode(AUTO_MODE);
+
+  //process variable or heading to be 90
+  controllerL.setSetPoint(90.0);
+  controllerR.setSetPoint(90.0);
+  
+ 
 
     while (true) {
         ble.waitForEvent();
-        val = ain.read_u16();
+            heading_ref = mag.getHeading();
+       if (heading_ref<0.0) heading_ref = heading_ref+360.00;
+       if (heading_ref>360.00) heading_ref = heading_ref -360.00;
+       controllerL.setProcessValue(heading_ref);
+       controllerR.setProcessValue(heading_ref);
+       float LPwr = controllerL.compute();
+       float RPwr = controllerR.compute();
+       //////////////////////////////////
+       //////////////////////////////////
+       //// WORKS BETTER THAN PID //////
+       //////////////////////////////////
+      ndr:
+      
+      if(refupdate == 1)
+      {
+       while(heading_ref != heading)
+       {
+        heading = mag.getHeading();
+       if (heading<0.0) heading_ref = heading+360.00;
+       if (heading>360.00) heading_ref = heading -360.00;
+       LP.write(.9);
+        }   
+       }
+      
+       
+       heading = heading_ref;
+       
+       for (int z =0;z<10;z++)
+       {
+       if (heading> heading_ref+2.0)
+       LP.write((heading - 90.0)/20.0);
+       else
+       LP.write(.0);   
+       wait(0.2);
+       
+       if (heading> heading_ref+2.0)
+       RP.write(( 90 - heading )/20.0);
+       else
+       RP.write(.0);   
+       wait(0.2);
+       LP.write(.7);   
+       RP.write(.7);   
+       wait(0.2);
+       
+       
+       }
+       /// heading is corrected after this line ///
+       /// calib may needed //
+
+    // val = ain.read_u16();
     // 22k+100K VOLTAGE DIVIDER AND 0.15 IS OFFSET   
-    batt = (122/22)*3.6*val/1023.0 - 0.15
-    dVal = batt;
-    dec = (int)(batt * 100) % 100;
+    // batt = (122/22)*3.6*val/1023.0 - 0.15
+    
+    //Update the process variable.
+    //Set the new output.
+ 
+      
+  if (blepush ==1)
+  {
+    
+    TEMP = 3.3*100*temperature.read_u16()/(5*1023); // 10 mV= 1 deg c, vcc =3.3 v, Opamp gain 5
+    dVal = TEMP;
+    dec = (int)(TEMP * 100) % 100;
 
     memset(result, 0, 100);
-    result[0] = (dVal / 10) + '0';
-    result[1] = (dVal % 10) + '0';
-    result[2] = '.';
-    result[3] = (dec / 10) + '0';
-    result[4] = (dec % 10) + '0';
-
-
+    
+    result[0] = (dVal / 100) + '0';
+    result[1] = ((dVal / 10)%10) + '0';
+    result[2] = (dVal % 10) + '0';
+    result[3] = '.';
+    result[4] = (dec / 10) + '0';
+    result[5] = (dec % 10) + '0';
     for (i=strlen(result)-1; i>=0; i--)
         putc(result[i], stdout);
-      
-            }
-}
+    
+    blePrintf("temperature ");
+    blePrintf(result);
+    blePrintf("\n");  
+
+    TURB = 3.3*10*turbidity.read_u16()/1023; // fix gain later
+    dVal = TURB;
+    dec = (int)(TURB * 100) % 100;
+
+    memset(result, 0, 100);
+    
+    result[0] = (dVal / 100) + '0';
+    result[1] = ((dVal / 10)%10) + '0';
+    result[2] = (dVal % 10) + '0';
+    result[3] = '.';
+    result[4] = (dec / 10) + '0';
+    result[5] = (dec % 10) + '0';
+    for (i=strlen(result)-1; i>=0; i--)
+        putc(result[i], stdout);
+    
+    blePrintf("turbidity ");
+    blePrintf(result);
+    blePrintf("\n");  
 
 
 
+    COND = 3.3*10*conductivity.read_u16()/1023; // fix gain later
+    dVal = COND;
+    dec = (int)(COND * 100) % 100;
+
+    memset(result, 0, 100);
+    
+    result[0] = (dVal / 100) + '0';
+    result[1] = ((dVal / 10)%10) + '0';
+    result[2] = (dVal % 10) + '0';
+    result[3] = '.';
+    result[4] = (dec / 10) + '0';
+    result[5] = (dec % 10) + '0';
+    for (i=strlen(result)-1; i>=0; i--)
+        putc(result[i], stdout);
+    
+    blePrintf("conductivity ");
+    blePrintf(result);
+    blePrintf("\n");  
+   blepush =0;
+   }
+      goto ndr;
+            }
+// end of while(1)            
+}
+// end of main()
+
+
+