I went on Fiverr, and paid someone to write some sketches for my project. What I got looks good, but functionally isn't what I was hoping for. Short story, I haven't been able to get a response from the person. Now I need to figure out what I am looking at. The base project is shown in the picture. The code for the Mega, Mini, and the ESP-01 are below. My main questions are as follows.
- The motor/motor controller/AS5600 are supposed to act as a servo, using P.I.D. controllers. With #define Motor1Pin 0, is that defining a single pin for control? How do I alter this to allow for 2 pin (directional) control?
- It looks like they used arrays for the 'tune' function. I need to be able to directly control the input values to each 'servo'. How can I do that?
- The ESP-01 connects over a wi-fi network to a PC running Node-Red. Is there a way to send an indication showing the connection is valid and active?
I am no programmer, but I can modify code if I understand what I need to do. I have a ton of questions, and I want to keep going. Also, I will still look at getting another programmer to finish coding all this.
Thanks for any help!
Code for Mega
#include "Vars.h"
void setup() {
for(int i = 0; i < 6; i++){
Motors[i].setup();
}
}
void tune(){
for(int i = 0; i < 6; i++){
Motors[i].tune();
}
}
void loop() {
for(int i = 0; i < 6; i++){
Motors[i].loop();
}
if(Serial.available()>0){//0,5.255
String data = Serial.readString();
int device_num = data.charAt(0);
if(device_num == '0'){
int comma = data.indexOf(',');
int period = data.indexOf('.');
int motor = data.substring(comma, period).toInt();
int speed = data.substring(period+1).toInt();
if(motor < 6){
Motors[motor].setSpeed(speed);
}else{
tune();
}
}
}
}
Vars.h
#include <Wire.h> //This is for i2C
#include <PID_v1.h>
#include <PID_AutoTune_v0.h>
#define TCAADDR 0x70
class encoder {
//Magnetic sensor things
int magnetStatus = 0; //value of the status register (MD, ML, MH)
int lowbyte; //raw angle 7:0
word highbyte; //raw angle 7:0 and 11:8
int rawAngle; //final raw angle
float degAngle; //raw angle in degrees (360/4096 * [value between 0-4095])
int quadrantNumber, previousquadrantNumber; //quadrant IDs
unsigned long prevMillis = 0;
float prevTurns = 0; //number of turns
float numberofTurns = 0; //number of turns
float correctedAngle = 0; //tared angle - based on the startup value
float startAngle = 0; //starting angle
float totalAngle = 0; //total absolute angular displacement
float previoustotalAngle = 0; //for the display printing
public:
encoder(){
}
void ReadRawAngle(){
//7:0 - bits
Wire.beginTransmission(0x36); //connect to the sensor
Wire.write(0x0D); //figure 21 - register map: Raw angle (7:0)
Wire.endTransmission(); //end transmission
Wire.requestFrom(0x36, 1); //request from the sensor
while(Wire.available() == 0); //wait until it becomes available
lowbyte = Wire.read(); //Reading the data after the request
//11:8 - 4 bits
Wire.beginTransmission(0x36);
Wire.write(0x0C); //figure 21 - register map: Raw angle (11:8)
Wire.endTransmission();
Wire.requestFrom(0x36, 1);
while(Wire.available() == 0);
highbyte = Wire.read();
//4 bits have to be shifted to its proper place as we want to build a 12-bit number
highbyte = highbyte << 8; //shifting to left
//What is happening here is the following: The variable is being shifted by 8 bits to the left:
//Initial value: 00000000|00001111 (word = 16 bits or 2 bytes)
//Left shifting by eight bits: 00001111|00000000 so, the high byte is filled in
//Finally, we combine (bitwise OR) the two numbers:
//High: 00001111|00000000
//Low: 00000000|00001111
// -----------------
//H|L: 00001111|00001111
rawAngle = highbyte | lowbyte; //int is 16 bits (as well as the word)
//We need to calculate the angle:
//12 bit -> 4096 different levels: 360° is divided into 4096 equal parts:
//360/4096 = 0.087890625
//Multiply the output of the encoder with 0.087890625
degAngle = rawAngle * 0.087890625;
//Serial.print("Deg angle: ");
//Serial.println(degAngle, 2); //absolute position of the encoder within the 0-360 circle
}
void correctAngle(){
//recalculate angle
correctedAngle = degAngle - startAngle; //this tares the position
if(correctedAngle < 0) //if the calculated angle is negative, we need to "normalize" it
{
correctedAngle = correctedAngle + 360; //correction for negative numbers (i.e. -15 becomes +345)
}
else
{
//do nothing
}
//Serial.print("Corrected angle: ");
//Serial.println(correctedAngle, 2); //print the corrected/tared angle
}
void checkQuadrant(){
/*
//Quadrants:
4 | 1
---|---
3 | 2
*/
//Quadrant 1
if(correctedAngle >= 0 && correctedAngle <=90)
{
quadrantNumber = 1;
}
//Quadrant 2
if(correctedAngle > 90 && correctedAngle <=180)
{
quadrantNumber = 2;
}
//Quadrant 3
if(correctedAngle > 180 && correctedAngle <=270)
{
quadrantNumber = 3;
}
//Quadrant 4
if(correctedAngle > 270 && correctedAngle <360)
{
quadrantNumber = 4;
}
//Serial.print("Quadrant: ");
//Serial.println(quadrantNumber); //print our position "quadrant-wise"
if(quadrantNumber != previousquadrantNumber) //if we changed quadrant
{
if(quadrantNumber == 1 && previousquadrantNumber == 4)
{
numberofTurns++; // 4 --> 1 transition: CW rotation
}
if(quadrantNumber == 4 && previousquadrantNumber == 1)
{
numberofTurns--; // 1 --> 4 transition: CCW rotation
}
//this could be done between every quadrants so one can count every 1/4th of transition
previousquadrantNumber = quadrantNumber; //update to the current quadrant
}
//Serial.print("Turns: ");
//Serial.println(numberofTurns,0); //number of turns in absolute terms (can be negative which indicates CCW turns)
//after we have the corrected angle and the turns, we can calculate the total absolute position
totalAngle = (numberofTurns*360) + correctedAngle; //number of turns (+/-) plus the actual angle within the 0-360 range
//Serial.print("Total angle: ");
//Serial.println(totalAngle, 2); //absolute position of the motor expressed in degree angles, 2 digits
}
void checkMagnetPresence(){
//This function runs in the setup() and it locks the MCU until the magnet is not positioned properly
while((magnetStatus & 32) != 32) //while the magnet is not adjusted to the proper distance - 32: MD = 1
{
magnetStatus = 0; //reset reading
Wire.beginTransmission(0x36); //connect to the sensor
Wire.write(0x0B); //figure 21 - register map: Status: MD ML MH
Wire.endTransmission(); //end transmission
Wire.requestFrom(0x36, 1); //request from the sensor
while(Wire.available() == 0); //wait until it becomes available
magnetStatus = Wire.read(); //Reading the data after the request
//Serial.print("Magnet status: ");
//Serial.println(magnetStatus, BIN); //print it in binary so you can compare it to the table (fig 21)
}
//Status register output: 0 0 MD ML MH 0 0 0
//MH: Too strong magnet - 100111 - DEC: 39
//ML: Too weak magnet - 10111 - DEC: 23
//MD: OK magnet - 110111 - DEC: 55
//Serial.println("Magnet found!");
//delay(1000);
}
void setup(){
checkMagnetPresence(); //check the magnet (blocks until magnet is found)
ReadRawAngle(); //make a reading so the degAngle gets updated
startAngle = degAngle; //update startAngle with degAngle - for taring
}
void loop(){
ReadRawAngle(); //ask the value from the sensor
correctAngle(); //tare the value
checkQuadrant();
}
int getSpeed(){
loop();
int speed = (numberofTurns - prevTurns)/(millis()-prevMillis);
prevMillis = millis();
return speed;
}
};
class MotorPID {
double Setpoint;
double Input;
double Output;
double Kp;
double Ki;
double Kd;
int num;
int pin;
int maxspeed;
PID myPID;
PID_ATune aTune;
encoder myEncoder;
public:
MotorPID(int n, int p) : myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT), aTune(&Input, &Output), myEncoder() {
num = n;
pin = p;
}
void setup(){
pinMode(pin, INPUT);
myPID.SetTunings(Kp,Ki,Kd);
myPID.SetSampleTime(1);
myPID.SetOutputLimits(-255, 255);
myPID.SetMode(1);
tcaselect(num);
myEncoder.setup();
}
void loop(){
tcaselect(num);
myEncoder.loop();
myPID.Compute();
analogWrite(pin, Output);
}
void tune(){
analogWrite(pin, 255);
delay(100);
maxspeed = myEncoder.getSpeed();
analogWrite(pin, 0);
aTune.SetNoiseBand(500);
aTune.SetOutputStep(1);
aTune.SetLookbackSec((int)20);
if(aTune.Runtime()){
Kp = aTune.GetKp();
Ki = aTune.GetKi();
Kd = aTune.GetKd();
myPID.SetTunings(Kp,Ki,Kd);
myPID.SetMode(1);
}
}
void setSpeed(int speed){
tcaselect(num);
int temp;
map(temp,-255, 255,-maxspeed,maxspeed);
Input = speed - temp;
loop();
}
int getSpeed(){
return Output;
}
void tcaselect(uint8_t i) {
if (i > 7) return;
Wire.beginTransmission(TCAADDR);
Wire.write(1 << i);
Wire.endTransmission();
}
};
MotorPID Motors[] = { MotorPID(0,Motor1Pin),MotorPID(1,Motor2Pin),MotorPID(2,Motor3Pin),MotorPID(3,Motor4Pin),MotorPID(4,Motor5Pin),MotorPID(5,Motor6Pin) };
Code for Mini
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
// you can also call it with a different address you want
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);
#define SERVOMIN 150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 600 // This is the 'maximum' pulse length count (out of 4096)
#define USMIN 600 // This is the rounded 'minimum' microsecond length
// based on the minimum pulse of 150
#define USMAX 2400 // This is the rounded 'maximum' microsecond length
// based on the maximum pulse of 600
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates
void setup() {
Serial.begin(9600);
pwm.begin();
pwm.setOscillatorFrequency(27000000);
pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates
delay(10);
}
void driveServo(int servo_num, int degrees){
int pulselength = map(degrees, 0, 180, SERVOMIN, SERVOMAX);
pwm.setPWM(servo_num, 0, pulselength);
}
void loop() {
if(Serial.available()>0){//1,16.120
String data = Serial.readString();
int device_num = data.charAt(0);
if(device_num == '1'){
int comma = data.indexOf(',');
int period = data.indexOf('.');
int motor = data.substring(comma, period).toInt();
int deg = data.substring(period+1).toInt();
}
}
}
Code for ESP - SSID and PW changed for security.
#include <ESP8266WiFi.h>
#include <PubSubClient.h>
#include "Arduino.h"
const char* ssid = "XXXXXXX";
const char* password = "XXXXXXX";
const IPAddress mqttServerIP(192,168,254,14);
const int httpPort = 1883;
const char* deviceID = "ESP8266";
WiFiClient wifiClient;
PubSubClient MQTTClient(wifiClient);
long lastMsgTime = 0;
char msg[64];
char topic[32];
int pulseCount = 0;
/*
* Code for MQTT reseves
* 0.{number of motor},{speed}
* 1.{number of servo},{degree}
*/
void mqttCallback(char* topic, byte* payload, unsigned int length){
memcpy(msg, payload,length);
msg[length] = '\0';
String message(msg);
int comma = message.indexOf(',');
int period = message.indexOf('.');
if(message.charAt(0) == '0'){
int motor_num = message.substring(period+1, comma).toInt();
String speed = message.substring(comma+1);
if(motor_num<6){
Serial.println("0."+String(motor_num)+","+speed);
}else{
Serial.println("0."+String(motor_num));
}
}else if(message.charAt(0) == '1'){
String servo_num = message.substring(period+1, comma);
String degree = message.substring(comma+1);
Serial.println("1."+servo_num+","+degree);
}
/*
Serial.print("Message recived in topic [");
Serial.print(topic);
Serial.print("] ");
Serial.println(msg);
*/
//Here is where to put stuff to act when somthing is recived
}
void wifiSetup(){
delay(10);
// We start by connecting to a WiFi network
/*
Serial.println();
Serial.print("Connecting to ");
Serial.println(ssid);
*/
WiFi.mode(WIFI_STA);
WiFi.begin(ssid, password);
while (WiFi.status() != WL_CONNECTED) {
delay(500);
//Serial.print(".");
}
/*
Serial.println("");
Serial.println("WiFi connected");
Serial.println("IP address: ");
Serial.println(WiFi.localIP());
*/
}
void mqttSetup(){
MQTTClient.setServer(mqttServerIP, 1883);
MQTTClient.setCallback(mqttCallback);
}
void setup() {
Serial.begin(9600);
wifiSetup();
mqttSetup();
}
void mqttLoop(){
while(!MQTTClient.connected()){
/*
Serial.print("Attempting to connect to MQTT broker at ");
Serial.println(mqttServerIP);
*/
if(MQTTClient.connect(deviceID)){
//Serial.println("Connected to MQTT broker");
/*
snprintf(topic,32,"ToHost/%s", deviceID);
snprintf(msg, 64, "CONNECT", deviceID);
MQTT.publish(topic,msg);
snprintf(topic, 32, "ToDevice/%s", deviceID);
MQTTClient.subscribe(topic);
*/
MQTTClient.subscribe("test");
}else{
Serial.print("Connection Failed");
delay(5000);
}
}
MQTTClient.loop();
}
void publish(char* message){
snprintf(topic, 32, "test", deviceID);
MQTTClient.publish(topic, message);
}
void loop() {
mqttLoop();
/*
if(Serial.available()>0){
String data = Serial.readString();
char temptopic[32];
data.toCharArray(temptopic, 32);
publish(temptopic);
}
*/
}