Loading...
play.demonicaether.com
Click to copy IP
Loading...
demonicaether.com/discord
Click to join
Avatar
Welcome to DemonicAether!
To join our community, please login or register!
Internet Explorer: Internet Explorer is not supported. Please upgrade to a more modern browser.
code
Topic Locked
DemonicAether OWNER
27 posts
27 topics
4 months ago
#include <SoftwareSerial.h>
#include <string.h>

SoftwareSerial BTSerial(9, 10); // rx pa tx

/*
 Step Delay: a milliseconds delay between the movement of each servo.  Allowed values from 10 to 30 msec.
   M1=base degrees. Allowed values from 0 to 180 degrees
   M2=shoulder degrees. Allowed values from 15 to 165 degrees
   M3=elbow degrees. Allowed values from 0 to 180 degrees
   M4=wrist vertical degrees. Allowed values from 0 to 180 degrees
   M5=wrist rotation degrees. Allowed values from 0 to 180 degrees
   M6=gripper degrees. Allowed values from 10 to 73 degrees. 10: the toungue is open, 73: the gripper is closed.
*/

struct Motor {
  int maxAngle, minAngle, currentAngle;
  String name;

  Motor(String _name, int _maxAngle, int _minAngle, int _currentAngle) {
    name = _name;
    maxAngle = _maxAngle;
    minAngle = _minAngle;
    currentAngle = _currentAngle;
  }

  void setAngle(int angle) {
    Serial.println("CALLED");
    currentAngle = angle;
  }

  int getAngle() {
    return currentAngle;
  }

};
Motor Base("Base",180,0,0);
Motor Shoulder("Shoulder",165,15,0);
Motor Elbow("Elbow",180,0,0);
Motor WristV("WristV",180,0,0);
Motor WristR("WristR",180,0,0);
Motor Grip("Grip",73,10,0);

Motor Motors[6] = { Base, Shoulder, Elbow, WristV, WristR, Grip };

void MoveMotor(String motorName, int angle) {
  for(int i=0; i<6; i++) {
    if(!(Motors[i].name == motorName)) Motors[i].setAngle(angle);
  }
}

void setup() {
  // put your setup code here, to run once
  BTSerial.begin(9600);
  Serial.begin(9600);
}

String msg = "";

void loop() {
  if(BTSerial.available() > 0) {
    char val = BTSerial.read();
    if(val == ';') {
      String firstParam = "";
      int secondParam = 0;
      bool first = true;

      for(int i=0; i<msg.length(); ++i) {
        if(msg[i] == ',') {
          first = false;
          continue;
        }

        if(first) firstParam += msg[i];
        else{
          secondParam = atoi(&msg[i]);
          break;
        }
      }
      Serial.println(firstParam);
      Serial.println(secondParam);
      MoveMotor(firstParam, secondParam);
      Serial.println(Motors[0].currentAngle);

      msg = "";
    }
    else {
      msg += val;
    }
  }
}