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

#include <Arduino.h>

#include <Automaton.h>
//#include "Atm_lien/Atm_stepper.h"
#include "Atm_Tstepper.h"
#include "Atm_TeensyStep.h"
#include <SPI.h>
#include <Ethernet.h>
#include <EthernetUdp.h>
#include <TeensyMAC.h>
#include <OSCMessage.h>

////////////////    Ethernet  /////////////////////////////
// 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, 204);    //local IP of Arduino/Teensy
//unsigned int localPort = 8888;      // local port to listen on (not needed for multicast)
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  ////////////////////////
/*move are always meant in full steps on the motor output (after reduction)
so all motors have 200steps per turn */
const int BARREL_SPEED = 8000 ;
const int BARREL_ACC = 800 ;
const int BARREL_DISTANCE = 10 ; //in full steps
const int BARREL_REDUCTION_RATIO = 26.85;
const int BARREL_MICROSTEP = 32;

static uint16_t BARREL_THRESHOLD[] = {5} ; //for sensor to trig
const int BARREL_INIBSENSOR = 5000;

const int PILL_SPEED = 10 ;
const int PILL_ACC = 1000 ;
const int PILL_DISTANCE = 200 ; //in full steps
const int PILL_MICROSTEP = 32;



Atm_TeensyStep barrel_step;
Stepper barrel_stepper(3, 2);
StepControl barrel_controller ;

Atm_comparator barrel_sensor;

Atm_TeensyStep pill_step;
Stepper pill_stepper(6, 5);
StepControl pill_controller ;

Atm_comparator pill_sensor ;


void barrel_homing(){
  barrel_step.move(BARREL_DISTANCE*BARREL_MICROSTEP*BARREL_REDUCTION_RATIO);
  automaton.delay(BARREL_INIBSENSOR); //let the current position go
  bool foundHome = false ;
  while(barrel_step.state() != barrel_step.STOPPING){
    // automaton.run();
    barrel_step.cycle();
    if(barrel_sensor.state()<BARREL_THRESHOLD[0]){
      barrel_step.emergencyStop();
      barrel_stepper.setPosition(0);
      foundHome = true ;
      break ;
    }
  }
  digitalWrite( 13, LOW );
  if (!foundHome){Serial.println("homing 0");}
  else{Serial.println("homing 1");}

}


void barrel_move(int stepRel){
  Serial.println(stepRel);
  barrel_step.move(stepRel);
}

void pill_move(int stepRel){
  Serial.println(stepRel);
  pill_step.move(stepRel);
}

void pill_next(){
  pill_move(200 * PILL_MICROSTEP);

  //make one turn, if nothing was seen by sensor try again up to 5 times
  bool foundPill = false ;
  while(pill_step.state() != barrel_step.STOPPING){
    automaton.run();
    if(pill_sensor.state()<BARREL_THRESHOLD[0]){foundPill = true ;}
  }
  if (!foundPill){Serial.println("pill 0");}
  else{Serial.println("pill 1");}

}

//////////////  Serial command machine  /////////////////////

char cmd_buffer[80];
Atm_command cmd;
char cmd1_buffer[80];
Atm_command cmd1;
enum { CMD_TEST, CMD_BARREL_MOVE, CMD_BARREL_HOME, CMD_BARREL_NEXT,
          CMD_PILL, CMD_PILL_MOVE };
const char cmdlist[] =
    "test barrel_move barrel_home barrel_next pill pill_move";
void cmd_callback( int idx, int v, int up ) {
  //int pin = atoi( cmd.arg( 1 ) );
  Serial.print(v);
  Serial.println("  in callback");
  switch ( v ) {
    case CMD_TEST:
      digitalWrite( 13, HIGH );
      Serial.println("in test");
      return;
    case CMD_BARREL_MOVE:
      barrel_move(atoi( cmd.arg( 1 ) ));
      return;
    case CMD_BARREL_HOME:
    // barrel_step.homing(0);
      barrel_homing();
      return;
    case CMD_BARREL_NEXT:
      barrel_move(BARREL_DISTANCE);
      if((!barrel_sensor.state())<BARREL_THRESHOLD[0]){barrel_homing();}
      return;
    case CMD_PILL:
      pill_next();
      return;
    case CMD_PILL_MOVE:
      pill_move(atoi( cmd.arg( 1 ) ));
      return;
  }
}


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

void setup() {
  //Configure and start ethernet module (not needed for feeder)
 //  SPI.setSCK(27);
 //  Ethernet.init(15);//(10)
 //  teensyMAC(mac);
 //  Ethernet.begin(mac, ip);              // start the Ethernet and UDP:
 // //  Udp.beginMulti(ipMulti, portMulti);   // for modified Arduino library
 //  Udp.beginMulticast(ipMulti, portMulti); // for modified Teensy Ethernet library

  //Start serial
  Serial.begin(115200); // higher Baud rate for faster refresh, CHANGE IN SERIAL MONITOR
  Serial1.begin(9600);
  delay(2000);
  Serial.println("Started");
  cmd.begin( Serial, cmd_buffer, sizeof( cmd_buffer ) )
    .trace(Serial)
    .list( cmdlist )
    .onCommand( cmd_callback );

  cmd1.begin( Serial1, cmd1_buffer, sizeof( cmd1_buffer ) )
    .trace(Serial)
    .list( cmdlist )
    .onCommand( cmd_callback );
  pinMode(13, OUTPUT);

  // pinMode(4, OUTPUT);
  // pinMode(4, LOW);
  barrel_step.trace( Serial );
  barrel_step.begin(barrel_stepper, barrel_controller, Udp, bndl, "/Y_top")
        .setEnablePin(4).enableReversed(1);
        //.setLimitType(3).setLimitPins(A3).limitThresholds(800, 1000, 0, 100).homing(1);
  barrel_stepper.setMaxSpeed(BARREL_SPEED);
  barrel_stepper.setAcceleration(BARREL_ACC);
  barrel_stepper.setInverseRotation(true);
  barrel_sensor.begin(A3, 50).threshold(BARREL_THRESHOLD, 1)
              .onChange( []( int idx, int v, int up ){Serial.println(up);} )
              .onChange( true, []( int idx, int v, int up ){Serial.println(up);} )
              .onChange( false, []( int idx, int v, int up ){Serial.println(up);} );

  pill_step.trace( Serial );
  pill_step.begin(pill_stepper, pill_controller,  Udp, bndl, "/Y_top")
        .setEnablePin(7).enableReversed(1);
  pill_stepper.setMaxSpeed(PILL_SPEED);
  pill_stepper.setAcceleration(PILL_ACC);
  pill_sensor.begin(A1, 50);
  //stepper.onOnchange(Machine &machine, optional int event = 0)
  //stepper.cycle(1000);
  //barrel_step.move(10000);
  // pill_step.move(10000);
  //controller.moveAsync(*stepper.motor);

}



void loop() {
  automaton.run();
  //Serial.println(barrel_sensor.state());

}