Wired uno for left knuckles (2020.06.21).txt

/*
  Author: GulJanjua
  Date  : Feb 10, 2020

  Software Requirements:
  ----------------------
  -Arduino IDE 1.0

  Hardware Requirements:
  ----------------------
  -Arduino
  -Pressure sensors
  -Gyroscope
  -Servo motors

  Project Requirents:
  -------------------
  • Game pad controller

*/

#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <SPI.h>
#include "mcp4261.h"
#include <SoftwareSerial.h>

SoftwareSerial mySerial(3, 2);

#define SensorV 15
#define angle1 20
#define angle2 87

#define Button 4

#define Sensor_BL A0
#define Sensor_BR A1
#define Sensor_SL A2
#define Sensor_SR A3

const int POT_0_CS = 6;
const int LOOP_DELAY = 200;
MCP4261 pot0 = MCP4261(POT_0_CS);

Adafruit_BNO055 bno1 = Adafruit_BNO055(-1, 0x28);
Adafruit_BNO055 bno2 = Adafruit_BNO055(-1, 0x29);

int speed = 0;
int angle = 0;

int y1_axis = 0, y2_axis = 0, y3_axis_1, y3_axis_2;
int y1_last = 0, y2_last = 0;
int y1_thres = 0, y2_thres = 0 , y3_thres_1 , y3_thres_2;
int sensorBL;
int sensorBR;
int sensorSL;
int sensorSR;

int prev_y1_axis = 0;
int y1_hold = 0;
unsigned long timer;
int start = 0;
byte step = 0;
String steps ,  laststep;
int freq;
int runC;
int press1 = 0 , press2 = 0;
byte pos;
int pox;

void setup() {
  Serial.begin(115200);
  mySerial.begin(9600);
  pinMode(Button , OUTPUT);
  digitalWrite(Button , LOW);
  if (!bno1.begin())
  {
    Serial.print("Ooops, no BNO055 1 detected ... Check your wiring or I2C ADDR!");
    while (1);
  }

  delay(1000);

  if (!bno2.begin())
  {
    Serial.print("Ooops, no BNO055 1 detected ... Check your wiring or I2C ADDR!");
    while (1);
  }
  delay(1000);
  bno1.setExtCrystalUse(true);
  bno2.setExtCrystalUse(true);
  delay(100);

  unsigned long timer;
  Serial.println("Calibration...");
  while (millis() - timer < 10000) {
    imu::Vector<3> euler1 = bno1.getVector(Adafruit_BNO055::VECTOR_EULER);
    imu::Vector<3> euler2 = bno2.getVector(Adafruit_BNO055::VECTOR_EULER);

    y1_thres = euler1.z();
    y2_thres = euler2.z();
    Serial.print(".");
    delay(100);
  }
  Serial.println("\nDone");
  Serial.println(String(y1_thres) + " " + String(y2_thres));
  delay(3000);


  // Setup SPI communications
  SPI.setDataMode(SPI_MODE0);
  SPI.setBitOrder(MSBFIRST);
  SPI.setClockDivider(SPI_CLOCK_DIV8);
  SPI.begin();

  // Initialize potentiometers
  pot0.initialize();
}

void loop() {
  imu::Vector<3> euler1 = bno1.getVector(Adafruit_BNO055::VECTOR_EULER);
  imu::Vector<3> euler2 = bno2.getVector(Adafruit_BNO055::VECTOR_EULER);

  y1_axis = euler1.z();
  y2_axis = euler2.z();
  receiveData();
  sensorBL = analogRead(Sensor_BL);
  sensorBR = analogRead(Sensor_BR);

  sensorSL = analogRead(Sensor_SL);
  sensorSR = analogRead(Sensor_SR);
  checkV();

  if (sensorSL > 120) {
    speed = map( sensorSL , 0 , 1023 , 0 , 255);
    if (sensorSL < 700) {
      //servo2.write(70, speed);
      pot0.setWiper0(20);
      press1 = 0;
    }
    else {
      //servo2.write(3 , speed);
      pot0.setWiper0(0);
      press1 = 0;
    }
  }
  else if (sensorSR > 120) {
    speed = map( sensorSR , 0 , 1023 , 0 , 255);

    if (sensorSR < 700) {
      //servo2.write(110, speed);
      pot0.setWiper0(240);
      press1 = 0;
    }
    else {
      //servo2.write(177 , speed);
      pot0.setWiper0(256);
      press1 = 0;
    }
  }

  if (sensorSL < 120 && sensorSR < 120 && pos == 0)  {
    //servo2.write(90 , 255);
    pot0.setWiper0(127);
    press1 = 0;
  }
  else if (sensorSL < 120 && sensorSR < 120 && pos == 1) {  //left down
    pot0.setWiper0(0);
    pot0.setWiper1(0);
  }
  else if (sensorSL < 120 && sensorSR < 120 && pos == 2) {  //left up
    pot0.setWiper0(0);
    pot0.setWiper1(256);
  }
  else if (sensorSL < 120 && sensorSR < 120 && pos == 3) {  //right down
    pot0.setWiper0(256);
    pot0.setWiper1(0);
  }
  else if (sensorSL < 120 && sensorSR < 120 && pos == 4) {  //right up
    pot0.setWiper0(256);
    pot0.setWiper1(256);
  }
  else if (sensorSL < 120 && sensorSR < 120 && pos == 5) {  //left
    pot0.setWiper0(0);
    pot0.setWiper1(127);
    press1 = 0;
  }
  else if (sensorSL < 120 && sensorSR < 120 && pos == 6) {  //right
    pot0.setWiper0(256);
    pot0.setWiper1(127);
    press1 = 0;
  }
  else if (sensorSL < 120 && sensorSR < 120 && pos == 7) {  //down
    pot0.setWiper0(127);
    pot0.setWiper1(0);
    press1 = 0;
  }
  else if (sensorSL < 120 && sensorSR < 120 && pos == 8) {  //up
    pot0.setWiper0(127);
    pot0.setWiper1(256);
    press1 = 0;
  }
  else if (sensorSL < 120 && sensorSR < 120 && pos == 9) {  //Jump
    pot0.setWiper0(127);
    pot0.setWiper1(256);
    press1 = 0;
  }
  else if (sensorSL < 120 && sensorSR < 120 && pos == 10) {  //Crawl
    pot0.setWiper0(127);
    pot0.setWiper1(0);
    press1 = 0;
  }


  //0 right  - 64 centr - 127 left

  if (sensorBL > 250 && pos == 0) {
    speed = map( sensorBL , 0 , 1023 , 0 , 255);
    if (sensorBL < 500) { //less pressure 90+20=110
      //servo1.write(85 , speed);
      pot0.setWiper1(240);
      angle = 95;
      press2 = 0;
    }
    else if (sensorBL > 500 && pos == 0) { //more pressre 90+90 = 180
      //servo1.write(105 , speed);
      pot0.setWiper1(256);
      angle = 105;
      press2 = 0;
    }
  }
  else if (sensorBR > 250 && pos == 0) {
    speed = map( sensorBR , 0 , 1023 , 0 , 255);
    if (sensorBR < 500) {  //less pressure
      //servo1.write(85 , speed);
      pot0.setWiper1(240);
      angle = 95;
      press2 = 0;
    }
    else if  (sensorBR > 500 && pos == 0) { //more pressre
      //servo1.write(105 , speed);
      pot0.setWiper1(256);
      angle = 105;
      press2 = 0;
    }
  }
  else if (pos == 0 && (start == 0 || millis() - timer > 800) && y1_axis > y1_thres - SensorV && y1_axis < y1_thres + SensorV && y2_axis > y2_thres - SensorV && y2_axis < y2_thres + SensorV && sensorBL < 200 && sensorBR < 200) //flat dead end no servo movement - center position -  can we separate on different lines the pressure sensor and accell so that I can set the dead end value separately for the press sens and acc ?
  {
    if (angle == 70 && runC > 0) {
      angle = 78;
      //servo1.write( 78 , 20);
      pot0.setWiper1(20);
      runC--;
      press2 = 0;
    }
    else {
      //servo1.write( 87 , 15);
      pot0.setWiper1(128);
      start = 1;
      angle = 87;
      press2 = 0;
    }
    //Serial.println("Servo goes to zero");
    step = 0;
    steps = "0";
  }
  else if ((y1_axis < y1_thres - SensorV) && (step == 0 || step == 2))
  {
    step = 1;
    steps = "1";
  }
  else if ((y2_axis < y2_thres - SensorV) && (step == 0 || step == 1))
  {
    step = 2;
    steps = "2";
  }

  if (!steps.equals(laststep) && pos == 0) {
    if ((step == 1 || step == 2)  &&  millis() - timer < 400) {
      if (freq > 1) {
        //servo1.write(70 , 20);
        pot0.setWiper1(0);
        angle = 70;
        press2 = 0;
      }
      else {
        //servo1.write(78 , 20);
        pot0.setWiper1(20);
        angle = 78;
        press2 = 0;
      }
      freq++;
      runC = 1;
    }
    else if ((step == 1 || step == 2 &&  millis() - timer > 675)) {
      //servo1.write(78 , 20);
      pot0.setWiper1(20);
      angle = 78;
      freq = 1;
      press2 = 0;
    }
    timer = millis();
  }

  if (press1 == 1 || press2 == 1)
    digitalWrite(Button , LOW);
  else
    digitalWrite(Button , HIGH);

  Serial.print("SL: " + String(sensorSL) + " SR: " + String(sensorSR));
  Serial.print(" BL: " + String(sensorBL) + " BR: " + String(sensorBR));
  Serial.print(" Y1: " + String(y1_axis) + " Y2: " + String(y2_axis));
  Serial.print("Pos: " + String(pos));
  Serial.print(" Y3x: " + String(y3_axis_1) + " Y3y: " + String(y3_axis_2));
  Serial.println(" Y3xx: " + String(y3_thres_1) + " Y3yy: " + String(y3_thres_2));

  laststep = steps;
  delay(80);
}

void receiveData() {
  if (mySerial.available()) {
    String data = mySerial.readStringUntil('\n');
    //Serial.println(data);
    y3_axis_1 = data.substring(0, data.indexOf(",")).toInt();
    data.remove(0, data.indexOf(",") + 1);
    y3_axis_2  = data.substring(0,  data.indexOf(",")).toInt();
    data.remove(0, data.indexOf(",") + 1);
    y3_thres_1  = data.substring(0,  data.indexOf(",")).toInt();
    data.remove(0, data.indexOf(",") + 1);
    y3_thres_2 = data.substring(0,  data.indexOf(",")).toInt();
    data.remove(0, data.indexOf(",") + 1);
    pox = data.substring(0,  data.length()).toInt();
  }
}

void checkV() {
  /*if (pox == 8)
    pos = 8;
    else if (pox == 9)
    pos = 9;
    else */

  if (y3_axis_1 > y3_thres_1 - 15 && y3_axis_1 < y3_thres_1 + 15 && y3_axis_2 > y3_thres_2 - 15 && y3_axis_2 < y3_thres_2 + 15)
    pos = 0; //center
  else if (y3_axis_1 > 0) {       //RIGHT
    if (y3_axis_2 > y3_thres_2 - 15 && y3_axis_2 < y3_thres_2 + 15)
      pos = 5;                    //RIGHT
    else if (y3_axis_2 > 0) {
      if (y3_axis_1 > y3_thres_1 - 15 && y3_axis_1 < y3_thres_1 + 15)
        pos = 7;                    //UP
      else
        pos = 1;                    //RIGHT UP
    }
    else if (y3_axis_2 < 0) {
      if (y3_axis_1 > y3_thres_1 - 15 && y3_axis_1 < y3_thres_1 + 15)
        pos = 8;                    //DOWN
      else
        pos = 2;                    //RIGHT DOWN
    }
  }
  else if (y3_axis_1 < 0) {       //LEFT
    if (y3_axis_2 > y3_thres_2 - 15 && y3_axis_2 < y3_thres_2 + 15)
      pos = 6;                    //LEFT
    else if (y3_axis_2 > 0) {
      if (y3_axis_1 > y3_thres_1 - 15 && y3_axis_1 < y3_thres_1 + 15)
        pos = 7;                    //UP
      else
        pos = 3;                    //UP LEFT
    }
    else if (y3_axis_2 < 0) {
      if (y3_axis_1 > y3_thres_1 - 15 && y3_axis_1 < y3_thres_1 + 15)
        pos = 8;                    //DOWN
      else
        pos = 4;                    //LEFT DOWN
    }
  }
}