HTequi/HTequi-firmware/src/blobcnc_low/main.cpp

#include <Arduino.h>

//Automaton and custom machines
#include <Automaton.h>
// #include "Atm_Teenstep.h"
// #include "Atm_Teenstep_OSC.h"
#include "Atm_AccelStepper.h"

//osc
#include <SPI.h>
#include <Ethernet.h>
#include <EthernetUdp.h>
#include <TeensyMAC.h>
#include <OSCBundle.h>



// Enter a MAC address and IP address for your controller below.
// The IP address will be dependent on your local network:
byte mac[] = {
 0x00, 0xAA, 0xBB, 0xCC, 0xDE, 0x02
};

// IPAddress ip(192, 168, 1, 104);    //local IP of Arduino/Teensy
// unsigned int localPort = 8000;      // local port to listen on (not needed for multicast)

// IPAddress outIp(192, 168, 1, 102);
// const unsigned int outPort = 9000;
IPAddress ipMulti(239, 200, 200, 200); // ipMidi Multicast address
unsigned int portMulti = 9977;   // ipMidi Mutlicast port 1


// buffers for receiving and sending data
byte packetBuffer[UDP_TX_PACKET_MAX_SIZE];       // buffer to hold incoming packet
byte sendBuffer1[] = {0x90, 0x14, 0x22};        // MIDI Note On to Multicast address
byte sendBuffer2[] = {0x80, 0x14, 0x00};        // MIDI Note Off to Multicast address

// An EthernetUDP instance to let us send and receive packets over UDP
EthernetUDP Udp;

///////////////////       OSC         ////////////////////////
OSCBundle bndl;

/////////////////// STEPPER machines  ////////////////////////
const int HIGH_SPEED = 5000 ;
const int HIGH_ACC = 800 ;
const int LOW_SPEED = 100 ;
const int LOW_ACC = 1000 ;

// Atm_Teenstep_OSC A_low_OSC;
// Atm_Teenstep A_low_step;
// Stepper A_low_stepper(8 , 7);
// StepControl A_low_controller ;
Atm_AccelStepper A_low_step ;

// Atm_Teenstep_OSC B_low_OSC;
// Atm_Teenstep B_low_step;
// Stepper B_low_stepper(32 , 31);
// StepControl B_low_controller ;
Atm_AccelStepper B_low_step ;

// Atm_Teenstep_OSC TrBr_low_OSC;
// Atm_Teenstep TrBr_low_step;
// Stepper TrBr_low_stepper(2 , 1);
// StepControl TrBr_low_controller ;
Atm_AccelStepper TrBr_low_step ;

// Atm_Teenstep_OSC LevBr_low_OSC;
// Atm_Teenstep LevBr_low_step;
// Stepper LevBr_low_stepper(5 , 4);
// StepControl LevBr_low_controller ;
Atm_AccelStepper LevBr_low_step ;



/////////////////// Servo and pump  ////////////////////////

const int filling_pwm_pin = 29;
const int brush_pwm_pin = 14;
const int crusher_pin = 16;


void motorsOSC(OSCMessage &msg){
  msg.dispatch("/filling", [](OSCMessage &msg){digitalWrite(filling_pwm_pin, msg.getInt(0));});
  msg.dispatch("/brush", [](OSCMessage &msg){analogWrite(brush_pwm_pin, msg.getInt(0));});
  msg.dispatch("/crusher", [](OSCMessage &msg){digitalWrite(crusher_pin, msg.getInt(0));});
}

//////////////  Setup  /////////////////////

void setup() {
 delay(2000);

 //  SERIAL
 Serial.begin(115200); // higher Baud rate for faster refresh, CHANGE IN SERIAL MONITOR
 //delay(2000);
 Serial.println("Started Low");

 // ETHERNET
 SPI.setSCK(27);
 Ethernet.init(10);//(10)
 teensyMAC(mac);

 Ethernet.begin(mac);              // start the Ethernet and UDP:
 Udp.beginMulticast(ipMulti, portMulti); // for modified Teensy Ethernet library
  // Udp.begin(localPort);

Serial.print("Started ethernet, IP is ");
Serial.println(Ethernet.localIP());

 //  STEPPERS

 A_low_step.begin(8, 7).setEnablePin(6).pinReversed(1)//.trace(Serial)
            .enable()
  .limitLow_set(2, 37, 0).limitLow_setThresholds(600, 715).limitLow_isHard(0)
  // .limitHigh_set(2, 37, 0).limitHigh_setThresholds(870, 1000).limitHigh_isHard(0)
  .setHomingSpeed(2000)
  .setMaxSpeed(HIGH_SPEED).setAcceleration(HIGH_ACC);
 A_low_step.onChangeposition([](int idx, int v, int up){bndl.add("/A_low/step").add(v).add(up);});
 A_low_step.onChangestate([](int idx, int v, int up){bndl.add("/A_low/state").add(v);});
 A_low_step.onOnlimitlow([](int idx, int v, int up){bndl.add("/A_low/limitLow").add(v);});
 A_low_step.onOnlimithigh([](int idx, int v, int up){bndl.add("/A_low/limitHigh").add(v);});
 A_low_step.onOntarget([](int idx, int v, int up){bndl.add("/A_low/onTarget").add(v);});

 B_low_step.begin(32 , 31).setEnablePin(30).pinReversed(0)//.trace(Serial)
            .enable()
  .limitLow_set(2, 35, 0).limitLow_setThresholds(600, 715).limitLow_isHard(0)
  // .limitHigh_set(2, 35, 0).limitHigh_setThresholds(870, 1000).limitHigh_isHard(0)
  .setHomingSpeed(2000)
  .setMaxSpeed(HIGH_SPEED).setAcceleration(HIGH_ACC);
 B_low_step.onChangeposition([](int idx, int v, int up){bndl.add("/B_low/step").add(v).add(up);});
 B_low_step.onChangestate([](int idx, int v, int up){bndl.add("/B_low/state").add(v);});
 B_low_step.onOnlimitlow([](int idx, int v, int up){bndl.add("/B_low/limitLow").add(v);});
 B_low_step.onOnlimithigh([](int idx, int v, int up){bndl.add("/B_low/limitHigh").add(v);});
 B_low_step.onOntarget([](int idx, int v, int up){bndl.add("/B_low/onTarget").add(v);});

 TrBr_low_step.begin(2, 1).setEnablePin(0).pinReversed(1)//.trace(Serial)
              .enable()
  .limitLow_set(1, 19, 1).limitLow_isHard(1)
  .limitHigh_set(1, 17, 1).limitHigh_isHard(1)
  .setHomingSpeed(2000)
  .setMaxSpeed(HIGH_SPEED).setAcceleration(HIGH_ACC);
 TrBr_low_step.onChangeposition([](int idx, int v, int up){bndl.add("/TrBr_low/step").add(v).add(up);});
 TrBr_low_step.onChangestate([](int idx, int v, int up){bndl.add("/TrBr_low/state").add(v);});
 TrBr_low_step.onOnlimitlow([](int idx, int v, int up){bndl.add("/TrBr_low/limitLow").add(v);});
 TrBr_low_step.onOnlimithigh([](int idx, int v, int up){bndl.add("/TrBr_low/limitHigh").add(v);});
 TrBr_low_step.onOntarget([](int idx, int v, int up){bndl.add("/TrBr_low/onTarget").add(v);});

 LevBr_low_step.begin(5 , 4).setEnablePin(3).pinReversed(1)//.trace(Serial)
                .enable()
  .limitLow_set(1, 15, 1).limitLow_isHard(1)
  .limitHigh_set(1, 39, 1).limitHigh_isHard(1)
  .setHomingSpeed(2000)
  .setMaxSpeed(HIGH_SPEED).setAcceleration(HIGH_ACC);
 LevBr_low_step.onChangeposition([](int idx, int v, int up){bndl.add("/LevBr_low/step").add(v).add(up);});
 LevBr_low_step.onChangestate([](int idx, int v, int up){bndl.add("/LevBr_low/state").add(v);});
 LevBr_low_step.onOnlimitlow([](int idx, int v, int up){bndl.add("/LevBr_low/limitLow").add(v);});
 LevBr_low_step.onOnlimithigh([](int idx, int v, int up){bndl.add("/LevBr_low/limitHigh").add(v);});
 LevBr_low_step.onOntarget([](int idx, int v, int up){bndl.add("/LevBr_low/onTarget").add(v);});

 //  Peristaltic pump
 pinMode(filling_pwm_pin, OUTPUT);
 pinMode(brush_pwm_pin, OUTPUT);
 pinMode(crusher_pin, OUTPUT);

 Serial.println("Ended setup");
 //TrBr_low_step.move(1000);
 Udp.beginPacket(ipMulti, portMulti);
 // Udp.beginPacket(outIp, outPort);
 bndl.add("/test").add("youpi");
 bndl.send(Udp); // send the bytes to the SLIP stream
 Udp.endPacket(); // mark the end of the OSC Packet
 bndl.empty(); //
}

void loop() {
  delay(1);
  Udp.beginPacket(ipMulti, portMulti);
  // Udp.beginPacket(outIp, outPort);
  automaton.run();
  bndl.send(Udp); // send the bytes to the SLIP stream
  Udp.endPacket(); // mark the end of the OSC Packet
  bndl.empty(); // empty the bundle to free room for a new one

  OSCMessage msgIn;
  int size;
  if( (size = Udp.parsePacket())>0)
  {
    while(size--)
      msgIn.fill(Udp.read());

     if(!msgIn.hasError()){
       //Serial.println("got OSC");
       msgIn.dispatch("/A_low/move", [](OSCMessage &msg){A_low_step.move(msg.getInt(0));});
       msgIn.dispatch("/A_low/moveTo", [](OSCMessage &msg){A_low_step.moveTo(msg.getInt(0));});
       msgIn.dispatch("/A_low/rotate", [](OSCMessage &msg){A_low_step.rotate(msg.getInt(0));});
       msgIn.dispatch("/A_low/stop", [](OSCMessage &msg){A_low_step.stop();});
       msgIn.dispatch("/A_low/emergency_stop", [](OSCMessage &msg){A_low_step.emergency_stop();});
       msgIn.dispatch("/A_low/homing", [](OSCMessage &msg){A_low_step.homing(msg.getInt(0));});
       msgIn.dispatch("/A_low/setMaxSpeed", [](OSCMessage &msg){A_low_step.setMaxSpeed(msg.getInt(0));});
       msgIn.dispatch("/A_low/setHomingSpeed", [](OSCMessage &msg){A_low_step.setHomingSpeed(msg.getInt(0));});
       msgIn.dispatch("/A_low/setAcceleration", [](OSCMessage &msg){A_low_step.setAcceleration(msg.getInt(0));});

       msgIn.dispatch("/B_low/move", [](OSCMessage &msg){B_low_step.move(msg.getInt(0));});
       msgIn.dispatch("/B_low/moveTo", [](OSCMessage &msg){B_low_step.moveTo(msg.getInt(0));});
       msgIn.dispatch("/B_low/rotate", [](OSCMessage &msg){B_low_step.rotate(msg.getInt(0));});
       msgIn.dispatch("/B_low/stop", [](OSCMessage &msg){B_low_step.stop();});
       msgIn.dispatch("/B_low/emergency_stop", [](OSCMessage &msg){B_low_step.emergency_stop();});
       msgIn.dispatch("/B_low/homing", [](OSCMessage &msg){B_low_step.homing(msg.getInt(0));});
       msgIn.dispatch("/B_low/setMaxSpeed", [](OSCMessage &msg){B_low_step.setMaxSpeed(msg.getInt(0));});
       msgIn.dispatch("/B_low/setHomingSpeed", [](OSCMessage &msg){B_low_step.setHomingSpeed(msg.getInt(0));});
       msgIn.dispatch("/B_low/setAcceleration", [](OSCMessage &msg){B_low_step.setAcceleration(msg.getInt(0));});

       msgIn.dispatch("/TrBr_low/move", [](OSCMessage &msg){TrBr_low_step.move(msg.getInt(0));});
       msgIn.dispatch("/TrBr_low/moveTo", [](OSCMessage &msg){TrBr_low_step.moveTo(msg.getInt(0));});
       msgIn.dispatch("/TrBr_low/rotate", [](OSCMessage &msg){TrBr_low_step.rotate(msg.getInt(0));});
       msgIn.dispatch("/TrBr_low/stop", [](OSCMessage &msg){TrBr_low_step.stop();});
       msgIn.dispatch("/TrBr_low/emergency_stop", [](OSCMessage &msg){TrBr_low_step.emergency_stop();});
       msgIn.dispatch("/TrBr_low/homing", [](OSCMessage &msg){TrBr_low_step.homing(msg.getInt(0));});
       msgIn.dispatch("/A_low/setMaxSpeed", [](OSCMessage &msg){TrBr_low_step.setMaxSpeed(msg.getInt(0));});
       msgIn.dispatch("/A_low/setHomingSpeed", [](OSCMessage &msg){TrBr_low_step.setHomingSpeed(msg.getInt(0));});
       msgIn.dispatch("/A_low/setAcceleration", [](OSCMessage &msg){TrBr_low_step.setAcceleration(msg.getInt(0));});

       msgIn.dispatch("/LevBr_low/move", [](OSCMessage &msg){LevBr_low_step.move(msg.getInt(0));});
       msgIn.dispatch("/LevBr_low/moveTo", [](OSCMessage &msg){LevBr_low_step.moveTo(msg.getInt(0));});
       msgIn.dispatch("/LevBr_low/rotate", [](OSCMessage &msg){LevBr_low_step.rotate(msg.getInt(0));});
       msgIn.dispatch("/LevBr_low/stop", [](OSCMessage &msg){LevBr_low_step.stop();});
       msgIn.dispatch("/LevBr_low/emergency_stop", [](OSCMessage &msg){LevBr_low_step.emergency_stop();});
       msgIn.dispatch("/LevBr_low/homing", [](OSCMessage &msg){LevBr_low_step.homing(msg.getInt(0));});
       msgIn.dispatch("/LevBr_low/setMaxSpeed", [](OSCMessage &msg){LevBr_low_step.setMaxSpeed(msg.getInt(0));});
       msgIn.dispatch("/LevBr_low/setHomingSpeed", [](OSCMessage &msg){LevBr_low_step.setHomingSpeed(msg.getInt(0));});
       msgIn.dispatch("/LevBr_low/setAcceleration", [](OSCMessage &msg){LevBr_low_step.setAcceleration(msg.getInt(0));});
       // B_low_OSC.onOSC(msgIn);
       // TrBr_low_OSC.onOSC(msgIn);
       // LevBr_low_OSC.onOSC(msgIn);
       motorsOSC(msgIn);

     }
  }
}