#include #include #include #include #include #include "esp32-hal-ledc.h" #include #include #include const byte DNS_PORT = 53; IPAddress apIP(192, 168, 4, 1); DNSServer dnsServer; WebSocketsServer webSocket = WebSocketsServer(81); WebServer server(80); const byte defaultArmPosition[6] = {90,90,90,125,90,90}; byte armPosition[6] = {90,90,125,90,90,90}; const char htmlInterface[] PROGMEM = "
Robot:

Turntable: 50

Joint1: 50

Joint2: 50

Joint3 50

Gripper: 50

Auxiliary:50

"; bool programmingMode = 0; bool playMode = 0; int eepromRestartIndex = 0; const unsigned long minimumProgramLength = 1; const unsigned long eepromSize =300; unsigned long eepromIndex; const unsigned long timeBetweenPlays = 600; unsigned long lastPlayed = millis(); // Pulse Mapping Parameters const int servoPulseMin = 2000; const int servoPulseMax = 8000; const int servoAngleMin = 0; const int servoAngleMax = 180; // servo parameters const int servoBounds[6][2] = { {0,180}, {0,180}, {0,180}, {0,180}, {0,180}, {0,180} }; const int servoPins[6] = {14,27,26,25,16,13}; void servosAttach(){ for(int i = 0; i<6;i++){ ledcSetup(i,50,16); ledcAttachPin(servoPins[i],i); } } void writeServos(){ for(int channel=0;channel<6;channel++){ if(armPosition[channel]servoBounds[channel][1]){ // if angle is less than min it is min armPosition[channel] = servoBounds[channel][1]; } int anglePulses = map(armPosition[channel],servoAngleMin,servoAngleMax,servoPulseMin,servoPulseMax); // map the angle to ledc pulses ledcWrite(channel,anglePulses); } } void beginPlaying(){ playMode =1; EEPROM.begin(eepromSize); eepromRestartIndex = EEPROM.read(0); eepromIndex = 2; } void endPlaying(){ playMode = 0; EEPROM.end(); } void beginProgramming(){ programmingMode = 1; EEPROM.begin(eepromSize); // set up eeprom for(int index = 0; index7){ EEPROM.end(); webSocket.broadcastTXT("Program Saved to Flash"); }else{ webSocket.broadcastTXT("Program Not Saved"); } } void saveState(){ for(int coordinate = 0;coordinate<6;coordinate++){ EEPROM.write(eepromIndex,armPosition[coordinate]); eepromIndex++; } } void handleRoot() { server.send(200, "text/html", htmlInterface); } void webSocketEvent(uint8_t num, WStype_t type, uint8_t * payload, size_t length) { switch(type) { case WStype_DISCONNECTED: Serial.printf("[%u] Disconnected!\n", num); break; case WStype_CONNECTED: { IPAddress ip = webSocket.remoteIP(num); Serial.printf("[%u] Connected from %d.%d.%d.%d url: %s\n", num, ip[0], ip[1], ip[2], ip[3], payload); // send message to client webSocket.sendTXT(num, "Connected"); } break; case WStype_TEXT: Serial.printf("[%u] get Text: %s\n", num, payload); Serial.println(); //webSocket.broadcastTXT(payload); if(payload[0]=='p'){ beginPlaying(); webSocket.broadcastTXT("Playing from EEPROM"); }else if(payload[0] =='r'){ endPlaying(); webSocket.broadcastTXT("Playing Ended"); }else if(payload[0]=='m'){ beginProgramming(); webSocket.broadcastTXT("Programming Begun"); }else if(payload[0]=='l'){ endProgramming(); }else if(payload[0]=='t'){ saveState(); webSocket.broadcastTXT("State Saved"); }else{ webSocket.broadcastTXT("Command Not Recognized"); } break; case WStype_BIN: Serial.printf("[%u] get binary length: %u\n", num, length); Serial.println(); for(int channel =0; channel<6;channel++){ armPosition[channel] = payload[channel]; } Serial.print(armPosition[0]); Serial.print(","); Serial.print(armPosition[1]); Serial.print(","); Serial.print(armPosition[2]); Serial.print(","); Serial.print(armPosition[3]); Serial.print(","); Serial.print(armPosition[4]); Serial.print(","); Serial.print(armPosition[5]); // webSocket.sendBIN(num, payload, length); break; } } void setup() { // Serial.begin(921600); Serial.begin(115200); //Serial.setDebugOutput(true); Serial.setDebugOutput(true); Serial.println(); WiFi.softAP("Robot Arm"); Serial.begin(115200); dnsServer.start(DNS_PORT, "*", apIP); server.on("/", handleRoot); server.begin(); Serial.println("HTTP server started"); servosAttach(); webSocket.begin(); webSocket.onEvent(webSocketEvent); } void loop() { dnsServer.processNextRequest(); server.handleClient(); if(playMode){//read position from eeprom and write it to the servos then set the virtual pins to what it read if((millis()-lastPlayed)>timeBetweenPlays){ for(int channel =0; channel<6;channel++){ armPosition[channel] = EEPROM.read(eepromIndex); eepromIndex++; } webSocket.broadcastBIN(armPosition, 6); if(eepromIndex == eepromRestartIndex){ eepromIndex = 2; } lastPlayed = millis(); } } webSocket.loop(); writeServos(); // push armPosition array to actual pwm }