Merge pull request #8 from ubcuas/ParachuteControl

Parachute Remote Control

sensorDemo/ParachuteControl/Reciever/Reciever.ino

@@ -0,0 +1,76 @@
+#include <Arduino.h>
+#include "Steering.h"
+#include "SDCard.h"
+#include <WiFi.h>
+#include <WebSocketsClient.h>
+
+#define SERIAL_PORT Serial
+
+const char* ssid = "ParachuteTX";
+const char* password = "UBC_UAS_2023";
+const char* ServerIP = "192.168.4.1";
+uint16_t ServerPort = 81;
+
+WebSocketsClient webSocket_Client;   //Websocket client object
+
+#define DELAY 20 //Delay between Reciever writes in ms
+
+typedef struct{
+  double Servo0;
+  double Servo1;
+} RemoteData;
+
+
+//Web socket event handler
+void webSocketEvent(WStype_t type, uint8_t * payload, size_t length){
+  if(type == WStype_DISCONNECTED){
+      Serial.printf("[WSc] Disconnected!\n");
+  } else if(type == WStype_CONNECTED){
+      Serial.printf("[WSc] Connected to url: %s\n", payload);
+      //Send the message to the server
+      webSocket_Client.sendTXT("Hello Server");
+  }  else if(type == WStype_BIN){
+    // Get the data
+    RemoteData* data = (RemoteData*)payload;
+    // print data
+    Serial.printf("Received data: Servo0: %lf, Servo1: %lf\n", data->Servo0, data->Servo1);
+    //Steering function
+    steering(data->Servo0);
+  }
+}
+
+void setup() {
+  Serial.begin(921600);
+
+  if(SDCard::SDcardInit() != SDCard::SDCARD_OK){
+    SERIAL_PORT.println("SD card failed");
+  }
+
+  //Setup WiFi
+  WiFi.begin(ssid, password);
+  while (WiFi.status() != WL_CONNECTED) {
+    delay(500);
+    Serial.print(".");
+  }
+  Serial.println("Connected to WiFi");
+
+  //Setup Websocket Client
+  webSocket_Client.begin(ServerIP, ServerPort, "/"); //Connect to the server
+  //Add Websocket event handler
+  webSocket_Client.onEvent(webSocketEvent);
+  //Retry connetction time
+  webSocket_Client.setReconnectInterval(5000);
+
+  motorSetup(); //Initialize two servo motors
+
+  delay(1000);
+}
+
+void loop(){
+
+    //Handle Websocket events
+    webSocket_Client.loop();
+
+
+
+}

sensorDemo/ParachuteControl/Reciever/SDCard.cpp

@@ -0,0 +1,63 @@
+#include <Arduino.h>
+#include "SDCard.h"
+
+SPIClass spi_1 = SPIClass(HSPI);      //define serial_port for serial
+
+namespace SDCard
+{
+    /**
+     * @brief Initialize the SD card
+     * We are using the SD card in SPI mode, so we need to connect the SD card to the SPI pins of the ESP32
+     * @return SDCardStatus 
+    */
+    SDCardStatus SDcardInit()
+    {
+        //configure the SD card detect pin as digital input:
+        pinMode(SD_DETECT, INPUT);
+        delay(10);
+
+        if(digitalRead(SD_DETECT)){
+          Serial.println("SD Card Not Plugged in!!\n");
+          return SDCARD_NOTCONNECTED;
+        }
+
+        spi_1.begin(SD_SCLK, SD_MISO, SD_MOSI, SD_CS);
+
+        if (!SD.begin(SD_CS, spi_1, 16000000))
+        {
+            Serial.println("initialization failed!");
+            return SDCARD_ERROR;
+        }
+        Serial.println("initialization done.");
+        return SDCARD_OK;
+    }
+
+    /**
+     * @brief Write data to the SD card
+     * We assume that the SD card is already initialized
+     * @param data the data to write to the SD card 
+     * @return SDCardStatus 
+     */
+    SDCardStatus SDcardWrite(const char data[])
+    { 
+        if(digitalRead(SD_DETECT)){
+          return SDCARD_NOTCONNECTED;
+        }
+        File file = SD.open("/data.txt", FILE_APPEND);
+        if (!file)
+        {
+            // Serial.println("Failed to open file for appending");
+            return SDCARD_ERROR;
+        }
+        if (file.print(data))
+        {
+            // Serial.println("File written");
+        }
+        else
+        {
+            // Serial.println("Write failed");
+        }
+        file.close();
+        return SDCARD_OK;
+    }
+} // namespace SDCard

sensorDemo/ParachuteControl/Reciever/SDCard.h

@@ -0,0 +1,30 @@
+#ifndef SDCARD_H
+#define SDCARD_H
+
+#include <Arduino.h>
+#include "FS.h"  
+#include "SD.h"
+#include "SPI.h"
+
+#define SD_CS 5
+#define SD_MOSI 23
+#define SD_MISO 19
+#define SD_SCLK 18
+#define SD_DETECT 17
+
+namespace SDCard
+{
+
+    typedef enum SDCardStatus {   //changed here
+    SDCARD_OK,
+    SDCARD_ERROR,
+    SDCARD_NOTCONNECTED
+    } SDCardStatus;
+
+    SDCardStatus SDcardInit();
+    SDCardStatus SDcardWrite(const char data[]); 
+} // namespace SDCard
+
+
+
+#endif

sensorDemo/ParachuteControl/Reciever/Steering.cpp

@@ -0,0 +1,44 @@
+#include "Steering.h"
+#include "Arduino.h"
+
+
+
+//Define servo motor classes
+Servo leftMotor;
+Servo rightMotor;
+
+void motorSetup(){
+  ESP32PWM::allocateTimer(0);
+	ESP32PWM::allocateTimer(1);
+	ESP32PWM::allocateTimer(2);
+	ESP32PWM::allocateTimer(3);
+  leftMotor.attach(LEFTPIN,MINUS,MAXUS);
+  rightMotor.attach(RIGHTPIN,MINUS,MAXUS);
+  leftMotor.setPeriodHertz(SERVOFREQ);      // Standard 50hz servo
+	rightMotor.setPeriodHertz(SERVOFREQ);      // Standard 50hz servo
+  leftMotor.write(90);
+  rightMotor.write(90);
+}
+
+void steering(double yaw){
+  //This part needs to be fixed. Euler->PID->steering. Change PID to be an angle or multiply value here to get angle?
+  //-x to +x ->Map from 0 to 180 ->change double to int
+  //-x to +x ->Map from 0 to 180 ->change double to int
+  //pv is the value that we observe, setpoint is what we want course to be.
+  //Set setpoint, then update pv.
+  int left_motor_value = map(yaw,-1000,1000,0,180);
+  int right_motor_value = map(yaw,-1000,1000,0,180);
+
+
+  Serial.print(left_motor_value);Serial.print("\t");
+  Serial.print(right_motor_value);Serial.print("\n");
+
+  leftMotor.write(left_motor_value);
+  rightMotor.write(right_motor_value);
+
+
+//Uncomment to get the values
+ // Serial.print(right_motor_value);Serial.print("\t");
+ // Serial.print(left_motor_value);Serial.print("\n");
+
+}

sensorDemo/ParachuteControl/Reciever/Steering.h

@@ -0,0 +1,20 @@
+/*Autonomous Parachute Steering Functon Header
+ *Author: Bobsy Narayan and Nischay Joshi
+ *Edited: Feb 19, 2022
+ */
+//Steering function declarations
+#ifndef _STEERING_H    
+#define _STEERING_H   
+#include <ESP32Servo.h>
+
+#define MINUS 450
+#define MAXUS 2450
+#define LEFTPIN 26
+#define RIGHTPIN 27
+#define ATTACHFREQ 1000
+#define SERVOFREQ 50
+
+void motorSetup();
+void steering(double yaw);
+
+#endif

sensorDemo/ParachuteControl/Transmitter/Transmitter.ino

@@ -0,0 +1,151 @@
+//Libraries:
+//For IMU 
+
+//For server communication
+#include <WiFi.h>
+#include <WebServer.h>
+#include <WebSocketsServer.h>
+
+#include "sensors.h"
+
+#define SERIAL_PORT Serial
+
+//Wifi credentials : Aim is to make a WiFiStation on ESP.
+const char* ssid = "ParachuteTX";
+const char* password = "UBC_UAS_2023";
+
+typedef struct{
+  double Servo0;
+  double Servo1;
+} RemoteData;
+
+Sensors::sensors mySensor_inst;
+Sensors::sensorData_t sensorData_inst;
+
+//For the Webserver stuff
+#define soft_AP 1 //if this is 1 then the ESP will be a soft AP otherwise it will be a station
+WebServer server(80); //Webserver object
+WebSocketsServer websocker_Server = WebSocketsServer(81); //Websocket object
+bool connected = false; //flag to check if a client is connected
+
+#define DELAY 20 //Delay between Transmitter updates in ms
+
+//This function is called when a client connects to the websocket
+void webSocketEvent(byte num, WStype_t type, uint8_t * payload, size_t length){
+  //Serial.printf("WebSocket %d received event: %d\n", num, type);
+  switch (type) {
+    case WStype_DISCONNECTED:
+      Serial.printf("WebSocket %d disconnected\n", num);
+      connected = false;
+      break;
+    case WStype_CONNECTED:
+      {
+        IPAddress ip = websocker_Server.remoteIP(num);
+        Serial.printf("WebSocket %d connected from %d.%d.%d.%d url: %s\n", num, ip[0], ip[1], ip[2], ip[3], payload);
+        connected = true;
+      }
+      break;
+    case WStype_TEXT:
+      Serial.printf("WebSocket %d received text: %s\n", num, payload);
+      // send message to client
+      break;
+    case WStype_BIN:
+      Serial.printf("WebSocket %d received binary length: %u\n", num, length);
+      // send message to client
+      websocker_Server.sendBIN(num, payload, length);
+      break;
+    case WStype_ERROR:
+      Serial.printf("WebSocket %d error\n", num);
+      break;
+    case WStype_FRAGMENT:
+      Serial.printf("WebSocket %d fragment\n", num);
+      break;
+    default:
+      Serial.printf("WebSocket %d unknown type\n", num);
+      break;
+  }
+}
+
+//Setup function
+void setup(void) {
+  //start serial communication
+  Serial.begin(921600);
+  while (!Serial)
+    delay(10);
+
+  // Initialize the Sensors
+  if(mySensor_inst.init() != Sensors::SENSORS_OK){
+    SERIAL_PORT.println("Sensor init failed");
+    while(1);
+  }
+
+  // Calibrate the barometer
+  if(mySensor_inst.CalibrateBarometerAltitude() != Sensors::SENSORS_OK){
+      SERIAL_PORT.println("Barometer calibration failed");
+  }
+
+  // Calibrate the IMU
+  if(mySensor_inst.CalibrateIMULinearAcceleration() != Sensors::SENSORS_OK){
+      SERIAL_PORT.println("IMU calibration failed");
+  }
+
+  //connect to WiFi, check which connection type is wanted
+    //Setup soft AP
+    WiFi.softAP(ssid, password);
+    IPAddress myIP = WiFi.softAPIP();
+    Serial.print("\nIP address: ");
+    Serial.println(myIP);
+    delay(5000);
+
+  // Start the server, when new client connects, we send the index.html page.
+  server.begin(); //intialize the server
+  websocker_Server.begin();  // intialze the websocket server
+  websocker_Server.onEvent(webSocketEvent);  // on a WS event, call webSocketEvent
+  Serial.println("Setup Done");
+  delay(2500);
+}
+
+void loop() {
+
+    // Handle all the server BS
+    server.handleClient(); // Listen for incoming clients
+    websocker_Server.loop(); // Listen for incoming websocket clients
+
+    //Update the IMU data
+    if(mySensor_inst.readData_noGPS(&sensorData_inst) == Sensors::SENSORS_OK){
+        //Send the data
+        SendData();
+    }
+  delay(DELAY); //delay for a bit
+}
+
+
+/*
+* Send the IMU Data through the websocket
+*/
+void SendData(){
+    if(!connected){
+        return;
+    }
+
+    // static RemoteData message = {-1000.0, -1000.0};
+    // message.Servo0 += 10;
+    // message.Servo1 += 10;
+
+    // if(message.Servo0 > 1000){
+    //     message.Servo0 = -1000;
+    // }
+    // if(message.Servo1 > 1000){
+    //     message.Servo1 = -1000;
+    // }
+
+    RemoteData message;
+    double pitch = sensorData_inst.imuData.EulerAngles.v1;
+    //convert that to a value between -1000 and 1000
+    message.Servo0 = map(pitch, -60, 60, -1000, 1000);
+    message.Servo1 = message.Servo0;
+
+    Serial.printf("Sending data: Servo0: %lf, Servo1: %lf\n", message.Servo0, message.Servo1);
+
+    websocker_Server.broadcastBIN((uint8_t*)&message, sizeof(message), false);
+}