Update hexapod lib for new pcb version

contributors.txt

@@ -1 +1,2 @@
 Luka Fućek
+Luka Dumančić

library.json

@@ -8,7 +8,7 @@
     "type": "git",
     "url": "https://github.com/stemi-education/stemi-hexapod"
   },
-  "version": "2.0.10",
+  "version": "3.0.0",
   "frameworks": "arduino",
   "platforms": "*"
 }

library.properties

@@ -1,8 +1,8 @@
 name=stemi-hexapod
-version=2.0.10
+version=3.0.0
 author=STEMI (info@stemi.education)
-maintainer=Luka Fucek (luka@stemi.education)
-sentence=Library for controlling STEMI Hexapod v2.
+maintainer=Luka Dumančić (luka.dumancic@stemi.education)
+sentence=Library for controlling STEMI Hexapod v3.
 paragraph=Includes drivers for BLE, servo, battery and everything else required to control STEMI hexapod robot.
 category=Device Control
 url=https://stemi.education

src/BatteryDriver.cpp

@@ -33,36 +33,47 @@ For additional information please check http://www.stemi.education.
 
 */
 
-
 #include "BatteryDriver.h"
+#include "SparkFun_MAX1704x_Fuel_Gauge_Arduino_Library.h"
+#include "Semaphore.h"
+
+SFE_MAX1704X lipo(MAX1704X_MAX17048);
 
-#define DEFAULT_VREF    1100        //Use adc2_vref_to_gpio() to obtain a better estimate
-#define NO_OF_SAMPLES   64          //Multisampling
+#define DEFAULT_VREF 1100 // Use adc2_vref_to_gpio() to obtain a better estimate
+#define NO_OF_SAMPLES 64  // Multisampling
 
 static esp_adc_cal_characteristics_t *adc_chars;
-static const adc_channel_t channel = ADC_CHANNEL_7;     //GPIO34 if ADC1, GPIO14 if ADC2
+static const adc_channel_t channel = ADC_CHANNEL_7; // GPIO34 if ADC1, GPIO14 if ADC2
 static const adc_atten_t atten = ADC_ATTEN_DB_0;
 static const adc_unit_t unit = ADC_UNIT_1;
 
-
 BatteryDriver::BatteryDriver()
 {
 
 	timeStarted = millis();
 	preferences.begin("my-app", false);
-	batteryPinCalibrationValue = preferences.getFloat("batCalibVal",0);
+	batteryPinCalibrationValue = preferences.getFloat("batCalibVal", 0);
 	Serial.println("Battery sense calibration data loaded: ");
-	Serial.println(batteryPinCalibrationValue);
+
+	delay(10);
+	lipo.enableDebugging();
+	while (lipo.begin() == false) // Connect to the MAX17043 using the default wire port
+	{
+		Serial.println(F("MAX1704X_MAX17048 not detected. Please check wiring. Freezing."));
+		delay(10);
+	}
+	lipo.quickStart();
+	lipo.setThreshold(5);
 
 	robot.battery.voltage = readBatteryVoltage();
-	for (uint8_t i = 0; i < 10; i++)
+	for (uint8_t i = 0; i < 1; i++)
 	{
 		checkState();
 #ifdef DEBUG_VOLTAGES
 		Serial.println(robot.battery.voltage);
-#endif // DEBUG_VOLTAGES	
+#endif // DEBUG_VOLTAGES
 	}
-	//scheck bat state
+	// scheck bat state
 	if (robot.battery.voltage > MIN_TURN_ON_VOLTAGE)
 		robot._setServoPower(1);
 	else
@@ -80,44 +91,55 @@ void BatteryDriver::checkState()
 	switch (robot.battery.state)
 	{
 	case BATTERY_LOW_STATE:
-		//from low to mid
+		// from low to mid
 		if (robot.battery.voltage > BATTERY_MID1_V + BATTERY_HYSTERESIS)
 		{
 			robot.battery.state = BATTERY_MID_STATE;
 			robot.battery.percentage = BATTERY_MID_P;
 		}
-		//from mid to low
-		if (robot.battery.voltage < BATTERY_LOW_V - BATTERY_HYSTERESIS/2)
+		// from mid to low
+		if (robot.battery.voltage < BATTERY_LOW_V - BATTERY_HYSTERESIS / 2)
 		{
 			robot.battery.state = BATTERY_EMPTY_STATE;
 			robot.battery.percentage = BATTERY_EMPTY_P;
-			//shut down servos and put to battery empty mode
+			// shut down servos and put to battery empty mode
 			robot._setServoPower(0);
 			robot.setMode(ROBOT_BATTERY_EMPTY_MODE);
 		}
 		break;
 	case BATTERY_MID_STATE:
-		//from mid to high
+		// from mid to high
 		if (robot.battery.voltage > BATTERY_MID2_V + BATTERY_HYSTERESIS)
 		{
 			robot.battery.state = BATTERY_HIGH_STATE;
 			robot.battery.percentage = BATTERY_HIGH_P;
 		}
-		//from mid to low
+		// from mid to low
 		if (robot.battery.voltage < BATTERY_MID1_V - BATTERY_HYSTERESIS)
 		{
 			robot.battery.state = BATTERY_LOW_STATE;
 			robot.battery.percentage = BATTERY_LOW_P;
 		}
 		break;
 	case BATTERY_HIGH_STATE:
-		//from high to mid
+		// from high to mid
 		if (robot.battery.voltage < BATTERY_MID2_V - BATTERY_HYSTERESIS)
 		{
 			robot.battery.state = BATTERY_MID_STATE;
 			robot.battery.percentage = BATTERY_MID_P;
 		}
 		break;
+	case BATTERY_EMPTY_STATE:
+		break;
+		// from empty to low
+		if (robot.battery.voltage > BATTERY_LOW_V)
+		{
+			robot.battery.state = BATTERY_LOW_STATE;
+			robot.battery.percentage = BATTERY_LOW_P;
+			robot._setServoPower(1);
+			robot.setMode(ROBOT_STANDBY_MODE);
+		}
+		break;
 	}
 
 #ifdef DEBUG_VOLTAGES
@@ -131,24 +153,25 @@ void BatteryDriver::checkState()
 	Serial.print(" ");
 	Serial.println(robot.battery.percentage);
 #endif // DEBUG_VOLTAGES
-
-
 }
 
 float BatteryDriver::readBatteryVoltage()
 {
-	float senVal = (float)(analogRead(BATTERY_STATUS_PIN));
-	return  (-0.000000000023926 * pow(senVal, 3) + 0.000000094746 * pow(senVal, 2) + 0.00074539 * senVal + 0.14925) * 2.0 + batteryPinCalibrationValue;
+	takeSemaphore();
+	float voltage = lipo.getVoltage();
+	giveSemaphore();
+	return voltage;
 }
 
 float BatteryDriver::LPFvoltage(float valueNew)
 {
 	float alpha = 0.5;
-	return alpha * robot.battery.voltage + (1 - alpha)*(valueNew);
+	return alpha * robot.battery.voltage + (1 - alpha) * (valueNew);
 }
 
 void BatteryDriver::calibrateBatteryPin()
 {
+	return;
 	if ((millis() - timeStarted) < 8000)
 	{
 		batteryPinCalibrationValue = 0;

src/BluetoothLowEnergy.cpp

@@ -38,8 +38,10 @@ For additional information please check http://www.stemi.education.
 #include <BLEUtils.h>
 #include <BLEServer.h>
 #include <BLE2904.h>
+#include <BLE2902.h>
 #include "BluetoothLowEnergy.h"
 #include "SharedData.h"
+#include "esp_ota_ops.h"
 
 class int8Callback : public BLECharacteristicCallbacks {
 private:
@@ -103,7 +105,20 @@ class robotNameCallback : public BLECharacteristicCallbacks {
 	robotNameCallback() {
 	}
 	void onWrite(BLECharacteristic* pCharacteristic) {
-			robot.storeName(pCharacteristic->getValue());
+		robot.storeName(pCharacteristic->getValue());
+		char nameDummy[20];
+		strcpy(nameDummy, pCharacteristic->getValue().c_str());
+		pCharacteristic->setValue((uint8_t*)nameDummy, 20);
+	}
+};
+
+class robotActionNameCallback : public BLECharacteristicCallbacks {
+private:
+public:
+	robotActionNameCallback() {
+	}
+	void onWrite(BLECharacteristic* pCharacteristic) {
+			robot.startAction(pCharacteristic->getValue());
 	}
 };
 
@@ -119,8 +134,74 @@ class int16Callback : public BLECharacteristicCallbacks {
 	}
 };
 
+class otaCallback : public BLECharacteristicCallbacks {
+private:
+	esp_ota_handle_t otaHandler = 0;
+	bool updateFlag = false;
+	bool readyFlag = false;
+	int bytesReceived = 0;
+	int timesWritten = 0;
+public:
+	otaCallback() {
+	}
+	void onWrite(BLECharacteristic* pCharacteristic) {
+		std::string rxData = pCharacteristic->getValue();
+		if (!updateFlag) { //If it's the first packet of OTA since bootup, begin OTA
+			Serial.println("BeginOTA");
+			esp_ota_begin(esp_ota_get_next_update_partition(NULL), OTA_SIZE_UNKNOWN, &otaHandler);
+			updateFlag = true;
+			bytesReceived = 0;
+		}
+		if (rxData.length() > 0)
+		{
+			esp_ota_write(otaHandler, rxData.c_str(), rxData.length());
+			bytesReceived += rxData.length();
+			if (rxData.length() != FULL_PACKET)
+			{
+				esp_ota_end(otaHandler);
+				Serial.println("EndOTA");
+				if (ESP_OK == esp_ota_set_boot_partition(esp_ota_get_next_update_partition(NULL))) {
+				delay(2000);
+				esp_restart();
+				}
+				else {
+				Serial.println("Upload Error");
+				}
+			}
+		}
+		Serial.println(bytesReceived);
+
+		uint8_t txData[5] = {1, 2, 3, 4, 5};
+		pCharacteristic->setValue((uint8_t*)txData, 5);
+		pCharacteristic->notify();
+	}
+};
+
+class nameCallback : public BLECharacteristicCallbacks {
+public:
+	nameCallback() {
+	}
+	void onWrite(BLECharacteristic* pCharacteristic) {
+		char nameDummy[20];
+		strcpy(nameDummy, robot.name.c_str());
+		char robotName[50] = "";
+		int robotNameLength = 0;
+		for (int i = 0; true; i++) {
+			robotName[i] = nameDummy[i];
+			robotNameLength += 1;
+			if (nameDummy[i] == '\0') {
+			break;
+			}
+		}
+		pCharacteristic->setValue((uint8_t*)robotName, robotNameLength);
+		pCharacteristic->notify();
+	}
+};
+
+
 
 BluetoothLowEnergy::BluetoothLowEnergy(std::string deviceName) {
+	deviceName = deviceName.substr(0, 29);
 	createBLEDevice(deviceName);
 	createBLEServer();
 	createMovementServiceWithCharacteristics();
@@ -255,8 +336,6 @@ void BluetoothLowEnergy::createPoseServiceWithCharacteristics() {
 
 void BluetoothLowEnergy::createParameterServiceWithCharacteristics() {
 	uint8_t init_data[2] = { 0, 0 };
-	char nameDummy[20];
-	strcpy(nameDummy, robot.name.c_str());
 
 	parameterService = server->createService(PARAMETER_SERVICE);
 
@@ -297,20 +376,13 @@ void BluetoothLowEnergy::createParameterServiceWithCharacteristics() {
 		BLECharacteristic::PROPERTY_READ
 	);
 
-	BLECharacteristic* nameCharacteristic = parameterService->createCharacteristic(
-		NAME_CHARACTERISTIC_UUID,
-		BLECharacteristic::PROPERTY_READ |
-		BLECharacteristic::PROPERTY_WRITE);
-
-	nameCharacteristic->setValue((uint8_t*)nameDummy, 20);
 	modeCharacteristic->setValue(&init_data[0], 1);
 	gaitIDCharacteristic->setValue(&init_data[0], 1);
 	universalDataCharacteristic->setValue(init_data, 1);
 	userSliderCharacteristic->setValue(&init_data[0], 1);
 	softwareVersionCharacteristic->setValue(robot.hexSwVersion, 3);
 	hardwareVersionCharacteristic->setValue(robot.hexHwVersion, 3);
 
-	nameCharacteristic->setCallbacks(new robotNameCallback());
 	modeCharacteristic->setCallbacks(new int8Callback(&robot.mode));
 	gaitIDCharacteristic->setCallbacks(new int8Callback(&robot.btInputData.gaitID));
 	universalDataCharacteristic->setCallbacks(new int8Callback(&robot.universalData[0]));
@@ -432,15 +504,53 @@ class batchCallback : public BLECharacteristicCallbacks {
 };
 
 void BluetoothLowEnergy::createBatchMovementServiceWithCharacteristic() {
+	char nameDummy[20];
+	strcpy(nameDummy, robot.name.c_str());
+    char robotName[50] = "";
+	int robotNameLength = 0;
+    for (int i = 0; true; i++) {
+        robotName[i] = nameDummy[i];
+		robotNameLength += 1;
+        if (nameDummy[i] == '\0') {
+          break;
+        }
+    }
+
 	batchService = server->createService(BATCH_SERVICE_UUID);
 	BLECharacteristic* batchCharacteristic = batchService->createCharacteristic(
 		BATCH_CHARACTERISTIC_UUID,
 		BLECharacteristic::PROPERTY_READ |
 		BLECharacteristic::PROPERTY_WRITE |
 		BLECharacteristic::PROPERTY_WRITE_NR);
+
+	BLECharacteristic* actionNameCharacteristic = batchService->createCharacteristic(
+		ACTION_NAME_CHARACTERISTIC_UUID,
+        BLECharacteristic::PROPERTY_WRITE |
+        BLECharacteristic::PROPERTY_INDICATE);
+
+	BLECharacteristic* pOtaCharacteristic = batchService->createCharacteristic(
+        OTA_CHARACTERISTIC_UUID,
+        BLECharacteristic::PROPERTY_NOTIFY | BLECharacteristic::PROPERTY_WRITE
+    );
+
+	BLECharacteristic* nameCharacteristic = batchService->createCharacteristic(
+		NAME_CHARACTERISTIC_UUID,
+		BLECharacteristic::PROPERTY_READ |
+		BLECharacteristic::PROPERTY_WRITE |
+		BLECharacteristic::PROPERTY_NOTIFY |
+        BLECharacteristic::PROPERTY_INDICATE);
+
+	nameCharacteristic->setValue((uint8_t*)robotName, robotNameLength);
+  	nameCharacteristic->setCallbacks(new nameCallback());
+
+	pOtaCharacteristic->addDescriptor(new BLEDescriptor((uint16_t)0x2902));
+  	pOtaCharacteristic->setCallbacks(new otaCallback());
 
 	uint8_t batchCommands[22];
 	batchCharacteristic->setValue(&batchCommands[0], 22);
+	actionNameCharacteristic->setValue("");
+
 	batchCharacteristic->setCallbacks(new batchCallback(&robot));
+	actionNameCharacteristic->setCallbacks(new robotActionNameCallback());
 	batchService->start();
 }

src/BluetoothLowEnergy.h

@@ -66,7 +66,7 @@ For additional information please check http://www.stemi.education.
 #define SOFTWARE_VERSION_CHARACTERISTIC_UUID "c526c005-97cd-46ca-992e-dae2b83c36e9"
 #define HARDWARE_VERSION_CHARACTERISTIC_UUID "2bbf68c9-2d73-4620-8eba-1758f6bef1b0"
 #define STEPHEIGHT_CHARACTERISTIC_UUID "e4fe2831-f7ae-4148-afd5-154a47f41530"
-#define NAME_CHARACTERISTIC_UUID "09769166-195f-495e-baa9-383c85211e97"
+#define NAME_CHARACTERISTIC_UUID "09769166-195f-495e-baa9-383c85211eff"
 
 #define LED_SERVICE "146d53ec-a92c-452c-9c2f-99bd7a6fbf8d"
 #define LEDDIRECTION_CHARACTERISTIC_UUID "fcd825b7-d05c-4829-b0e0-3b0fa8b4caff"
@@ -81,6 +81,12 @@ For additional information please check http://www.stemi.education.
 
 #define BATCH_SERVICE_UUID "651707bb-61f7-4338-b5a9-c36e02c8e2f1"
 #define BATCH_CHARACTERISTIC_UUID "da86113b-ae65-472e-ac2e-ad7d8a00feac"
+#define ACTION_NAME_CHARACTERISTIC_UUID "acf4610b-f162-45e2-9cac-cd6ca32b9488"
+#define OTA_CHARACTERISTIC_UUID "9c2f0ede-7162-4504-8ecf-e0a2357d20f5"
+
+// BleOta
+#define FULL_PACKET 512
+#define CHARPOS_UPDATE_FLAG 5
 
 class BluetoothLowEnergy {
 public:

src/Body.cpp

@@ -146,7 +146,7 @@ void Body::setGaitUpDown(double gaitArray[12])
 	for (int i = 0; i < nWalkingLegs; i++)
 	{
 		legs[walkingLegsMap[i]].setGaitUpDown(gaitArray[walkingLegsMap[i] * 2] * PI / 3.0, gaitArray[walkingLegsMap[i] * 2 + 1] * PI / 3.0);
-		//TODO zasto se upisuju krivi FIjevi? ispiši i quadWaveBase pa provjeri
+		//TODO zasto se upisuju krivi FIjevi? ispi�i i quadWaveBase pa provjeri
 	}
 }