top : changed limit pins, need check

low : base code

HTequi-firmware/src/blobcnc_low/main.cpp

@@ -1,14 +1,19 @@
+
 #include <Arduino.h>
 
+//Automaton and custom machines
 #include <Automaton.h>
-#include "Atm_lien/Atm_stepper.h"
-#include "Atm_lien/Atm_TeensyStep.h"
+#include "Atm_Teenstep.h"
+#include "Atm_Teenstep_OSC.h"
 
+//osc
 #include <SPI.h>
 #include <Ethernet.h>
 #include <EthernetUdp.h>
 #include <TeensyMAC.h>
-#include <OSCMessage.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[] = {
@@ -30,170 +35,144 @@ byte sendBuffer2[] = {0x80, 0x14, 0x00};        // MIDI Note Off to Multicast ad
 // An EthernetUDP instance to let us send and receive packets over UDP
 EthernetUDP Udp;
 
-// Atm_stepper stepper;
-Atm_TeensyStep stepper;
-Stepper A_stepper(2, 1);
-StepControl controller ;
-
-class Atm_blink : public Machine {
-
- public:
-   Atm_blink( void ) : Machine() {};
-   short pin;
-   atm_timer_millis timer;
-   OSCMessage _msg ;
-   EthernetUDP* _udpRef ;
-   //Atm_blink& onOSC(OSCMessage& msg );
-
-   enum { IDLE, LED_ON, LED_OFF }; // STATES
-   enum { EVT_TIMER, EVT_ON, EVT_OFF, ELSE }; // EVENTS
-   enum { ENT_ON, ENT_OFF }; // ACTIONS
-
-   Atm_blink & begin( int attached_pin, uint32_t blinkrate, EthernetUDP& udpRef  ) {
-     const static state_t state_table[] PROGMEM = {
-       /*            ON_ENTER    ON_LOOP  ON_EXIT  EVT_TIMER  EVT_ON  EVT_OFF  ELSE */
-       /* IDLE    */  ENT_OFF,        -1,      -1,        -1, LED_ON,      -1,   -1,
-       /* LED_ON  */   ENT_ON,        -1,      -1,   LED_OFF,     -1,    IDLE,   -1,
-       /* LED_OFF */  ENT_OFF,        -1,      -1,    LED_ON,     -1,    IDLE,   -1,
-     };
-     Machine::begin( state_table, ELSE );
-     pin = attached_pin;
-     timer.set( blinkrate );
-     pinMode( pin, OUTPUT );
-     this->_udpRef =   &udpRef;
-
-     return *this;
-   }
-
-   int event( int id ) {
-     switch ( id ) {
-       case EVT_TIMER :
-         return timer.expired( this );
 
-   }
- }
+/////////////////// STEPPER machines  ////////////////////////
+const int HIGH_SPEED = 800 ;
+const int HIGH_ACC = 800 ;
+const int LOW_SPEED = 100 ;
+const int LOW_ACC = 1000 ;
 
-   void action( int id ) {
+Atm_Teenstep_OSC A_low_OSC;
+Atm_Teenstep A_low_step;
+Stepper A_low_stepper(8 , 7);
+StepControl A_low_controller ;
 
-     switch ( id ) {
-       case ENT_ON :
-         digitalWrite( pin, HIGH );
+Atm_Teenstep_OSC B_low_OSC;
+Atm_Teenstep B_low_step;
+Stepper B_low_stepper(32 , 31);
+StepControl B_low_controller ;
 
+Atm_Teenstep_OSC TrBr_low_OSC;
+Atm_Teenstep TrBr_low_step;
+Stepper TrBr_low_stepper(2 , 1);
+StepControl TrBr_low_controller ;
 
-         return;
+Atm_Teenstep_OSC LevBr_low_OSC;
+Atm_Teenstep LevBr_low_step;
+Stepper LevBr_low_stepper(5 , 4);
+StepControl LevBr_low_controller ;
 
-       case ENT_OFF :
-         digitalWrite( pin, LOW );
 
-         return;
-     }
-   }
-
-   Atm_blink& trigger( int event ) {
-     Machine::trigger( event );
-     return *this;
-   }
-
-   /* Optionally override the default state() method
-    * Control what the machine returns when another process requests its state
-    */
-    // Atm_led& Atm_led::on( void ) {
-    //   trigger( EVT_ON );
-    //   return *this;
-
-
-   Atm_blink& onOSC(OSCMessage& msg ){
-     Serial.println("OSC");
-     if(msg.fullMatch("/blink")){
-       Serial.println("ouais gros");
-       // int blinkstate = _msg.getInt(0);
-       // blinkstate ? trigger(EVT_ON) : trigger(EVT_OFF);
-      //  _msg = new OSCMessage("/on");
-      // _msg.add("youpi");
-      // _udpRef->beginPacket(ipMulti, portMulti);
-      // _msg.send(*_udpRef); // send the bytes to the SLIP stream
-      // _udpRef->endPacket(); // mark the end of the OSC Packet
-      // _msg.empty();
-       return *this;
-     }
-   }
+/////////////////// Servo and pump  ////////////////////////
 
+const int peristaltic_pwm_pin = 3;
+const int peristaltic_dir_pin = 2;
+bool peristaltic_direction = 0 ;
 
-};
+Servo pill_trap ;
 
-Atm_blink led;
-
-void test(OSCMessage &msg, int addrOffset){
-  Serial.println("test");
-msg.getInt(0)?led.trigger( led.EVT_ON ):led.trigger( led.EVT_OFF );
+void motorsOSC(OSCMessage &msg){
+  msg.dispatch("/pill_trap", [](OSCMessage &msg){pill_trap.write(msg.getFloat(0));});
+  msg.dispatch("/peristaltic", [](OSCMessage &msg){analogWrite(peristaltic_pwm_pin, msg.getInt(0));});
 }
 
-
+//////////////  Setup  /////////////////////
 
 void setup() {
- //SPI.setSCK(27);
- Ethernet.init(15);//(10)
+ SPI.setSCK(27);
+ Ethernet.init(10);//(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
+
+ //  SERIAL
  Serial.begin(115200); // higher Baud rate for faster refresh, CHANGE IN SERIAL MONITOR
+ delay(2000);
+ Serial.println("Started Low");
+
+ //  STEPPERS
+ A_low_step.trace( Serial );
+ A_low_step.begin(A_low_stepper, A_low_controller)
+           .setEnablePin(6)//.enableReversed(1);//.enable(1);
+           .setLimitType(3).setLimitPins(A16).limitReversed(false).limitThresholds(600, 750, 950, 1200);
+ A_low_stepper.setMaxSpeed(HIGH_SPEED);
+ A_low_stepper.setAcceleration(HIGH_ACC);
+ A_low_stepper.setInverseRotation(true);
+ A_low_OSC.begin(A_low_step, Udp, bndl, "/A_low");
+ A_low_step.onChangeposition([](int idx, int v, int up){bndl.add("/A_low_OSC/step").add(v);});
+ A_low_step.onChange([](int idx, int v, int up){bndl.add("/A_low_OSC/state").add(v);});
+ A_low_step.onLimitlow([](int idx, int v, int up){bndl.add("/A_low_OSC/limitLow").add(v);});
+ A_low_step.onLimithigh([](int idx, int v, int up){bndl.add("/A_low_OSC/limitHigh").add(v);});
+
+ B_low_step.trace( Serial );
+ B_low_step.begin(B_low_stepper, B_low_controller)
+           .setEnablePin(30)//.enableReversed(1);//.enable(1);
+           .setLimitType(3).setLimitPins(A14).limitReversed(false).limitThresholds(600, 750, 950, 1200);
+ B_low_stepper.setMaxSpeed(HIGH_SPEED);
+ B_low_stepper.setAcceleration(HIGH_ACC);
+ B_low_stepper.setInverseRotation(true);
+ B_low_OSC.begin(B_low_step, Udp, bndl, "/B_low");
+ B_low_step.onChangeposition([](int idx, int v, int up){bndl.add("/B_low_OSC/step").add(v);});
+ B_low_step.onChange([](int idx, int v, int up){bndl.add("/B_low_OSC/state").add(v);});
+ B_low_step.onLimitlow([](int idx, int v, int up){bndl.add("/B_low_OSC/limitLow").add(v);});
+ B_low_step.onLimithigh([](int idx, int v, int up){bndl.add("/B_low_OSC/limitHigh").add(v);});
+
+ TrBr_low_step.trace( Serial );
+ TrBr_low_step.begin(TrBr_low_stepper, TrBr_low_controller)
+           .setEnablePin(0)//.enableReversed(1);//.enable(1);
+           .setLimitType(2).setLimitPins(17, 15).limitReversed(false).limitThresholds(600, 750, 950, 1200);
+ TrBr_low_stepper.setMaxSpeed(HIGH_SPEED);
+ TrBr_low_stepper.setAcceleration(HIGH_ACC);
+ TrBr_low_stepper.setInverseRotation(true);
+ TrBr_low_OSC.begin(TrBr_low_step, Udp, bndl, "/TrBr_low");
+ TrBr_low_step.onChangeposition([](int idx, int v, int up){bndl.add("/TrBr_low_OSC/step").add(v);});
+ TrBr_low_step.onChange([](int idx, int v, int up){bndl.add("/TrBr_low_OSC/state").add(v);});
+ TrBr_low_step.onLimitlow([](int idx, int v, int up){bndl.add("/TrBr_low_OSC/limitLow").add(v);});
+ TrBr_low_step.onLimithigh([](int idx, int v, int up){bndl.add("/TrBr_low_OSC/limitHigh").add(v);});
+
+ LevBr_low_step.trace( Serial );
+ LevBr_low_step.begin(LevBr_low_stepper, LevBr_low_controller)
+           .setEnablePin(3)//.enableReversed(1);//.enable(1);
+           .setLimitType(2).setLimitPins(39, 37).limitReversed(false).limitThresholds(600, 750, 950, 1200);
+ LevBr_low_stepper.setMaxSpeed(HIGH_SPEED);
+ LevBr_low_stepper.setAcceleration(HIGH_ACC);
+ LevBr_low_stepper.setInverseRotation(true);
+ LevBr_low_OSC.begin(LevBr_low_step, Udp, bndl, "/LevBr_low");
+ LevBr_low_step.onChangeposition([](int idx, int v, int up){bndl.add("/LevBr_low_OSC/step").add(v);});
+ LevBr_low_step.onChange([](int idx, int v, int up){bndl.add("/LevBr_low_OSC/state").add(v);});
+ LevBr_low_step.onLimitlow([](int idx, int v, int up){bndl.add("/LevBr_low_OSC/limitLow").add(v);});
+ LevBr_low_step.onLimithigh([](int idx, int v, int up){bndl.add("/LevBr_low_OSC/limitHigh").add(v);});
+
+ //  Peristaltic pump
+ pinMode(peristaltic_dir_pin, OUTPUT);
+ digitalWrite(peristaltic_dir_pin, peristaltic_direction);
+ pinMode(peristaltic_pwm_pin, OUTPUT);
+ analogWrite(peristaltic_pwm_pin, 50);
 
- // led.begin( 14, 200, Udp);        // Setup a blink machine on pin 4
- // led.trigger( led.EVT_ON );  // Turn it on
-
-  delay(1000);
-
-  // stepper.trace( Serial );
-  // Serial.println("Started");
-  // stepper.begin(A_stepper);
-  // stepper.cycle(1000);
-  // Serial.println("Started");
-  // stepper.gotoStep(1000);
-  // controller.moveAsync(*stepper.motor);
-  // delay(1000);
-  // controller.moveAsync(*stepper.motor);
-  // delay(10000);
-  // stepper.motor->setMaxSpeed(30000);
-  // stepper.motor->setAcceleration(10000);
-  // stepper.gotoStep(-100000);
-  // controller.moveAsync(*stepper.motor);
-  // Serial.println("Started");
-  // stepper.trigger(stepper.EVT_GOTO);
-  // pinMode(00, OUTPUT);
-  // digitalWrite(0, HIGH);
 }
 
 void loop() {
- //  automaton.run();
- // OSCMessage msgIn;
- // int size;
-  Serial.println("msgIn");
- // if( (size = Udp.parsePacket())>0)
- // {
- //   while(size--)
- //     msgIn.fill(Udp.read());
- //
- //    if(!msgIn.hasError()){
- //      // msgIn.route("/blink", test);
- //      // led.onOSC(msgIn);
- //    }
- //
- //
- // }
-
- OSCMessage msg("/analog/0");
- msg.add((int32_t)analogRead(0));
-
- Udp.beginPacket(ipMulti, portMulti);
-   msg.send(Udp); // send the bytes to the SLIP stream
- Udp.endPacket(); // mark the end of the OSC Packet
- msg.empty(); // free space occupied by message
-
- // led.cycle();
-
- delay(100);
- //UDPsend();   // sending of packets; do not use with UDPreceive because it's including delay()!!
- //UDPreceive();
-
-}
+  automaton.run();
+  Udp.beginPacket(ipMulti, portMulti);
+  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.route("/blink", test);
+       A_low_OSC.onOSC(msgIn);
+       B_low_OSC.onOSC(msgIn);
+       feederOSC(msgIn);
+       motorsOSC(msgIn);
+       // B_low_step.onOSC(msgIn);
+       // peristaltic.onOSC(msgIn);
+     }
+  }

HTequi-firmware/src/blobcnc_top/main.cpp

@@ -3,12 +3,8 @@
 
 //Automaton and custom machines
 #include <Automaton.h>
-// #include "Atm_lien/Atm_stepper.h"
-// #include "Atm_TeensyStep.h"
-// #include "Atm_Tstepper_OSC.h"
 #include "Atm_Teenstep.h"
 #include "Atm_Teenstep_OSC.h"
-#include "Atm_out.h"
 
 //osc
 #include <SPI.h>
@@ -56,10 +52,6 @@ Stepper Y_top_stepper(22 , 21);
 StepControl Y_top_controller ;
 
 
-void testCallback(){
-  // printPosition
-}
-
 /////////////////// Servo and pump  ////////////////////////
 
 const int peristaltic_pwm_pin = 3;
@@ -68,6 +60,11 @@ bool peristaltic_direction = 0 ;
 
 Servo pill_trap ;
 
+void motorsOSC(OSCMessage &msg){
+  msg.dispatch("/pill_trap", [](OSCMessage &msg){pill_trap.write(msg.getFloat(0));});
+  msg.dispatch("/peristaltic", [](OSCMessage &msg){analogWrite(peristaltic_pwm_pin, msg.getInt(0));});
+}
+
 /////////////////// Feeders serial communication  ////////////////////////
 char feeder1_buffer[80];
 Atm_command feeder1;
@@ -140,10 +137,7 @@ void feederOSC(OSCMessage &msg){
   msg.dispatch("/feeder2/pill_move", [](OSCMessage &msg){Serial2.print("pill_move ");Serial2.print(msg.getInt(0));Serial2.println(" ");Serial.println(msg.getInt(0));});
 }
 
-void motorsOSC(OSCMessage &msg){
-  msg.dispatch("/pill_trap", [](OSCMessage &msg){pill_trap.write(msg.getFloat(0));});
-  msg.dispatch("/peristaltic", [](OSCMessage &msg){analogWrite(peristaltic_pwm_pin, msg.getInt(0));});
-}
+
 
 
 //////////////  Setup  /////////////////////
@@ -168,7 +162,7 @@ void setup() {
   X_top_step.trace( Serial );
   X_top_step.begin(X_top_stepper, X_top_controller)
             .setEnablePin(18)//.enableReversed(1);//.enable(1);
-            .setLimitType(3).setLimitPins(A3).limitReversed(false).limitThresholds(600, 750, 950, 1200);
+            .setLimitType(2).setLimitPins(16, 17).limitReversed(true);
   X_top_stepper.setMaxSpeed(HIGH_SPEED);
   X_top_stepper.setAcceleration(HIGH_ACC);
   X_top_stepper.setInverseRotation(true);
@@ -183,7 +177,7 @@ void setup() {
   Y_top_step.begin(Y_top_stepper, Y_top_controller)
             .setEnablePin(23).enableReversed(1)
             //limit pin is on 7 and 8
-             .setLimitType(1).setLimitPins(16).limitReversed(true);
+             .setLimitType(2).setLimitPins(6, 7).limitReversed(true);
   Y_top_stepper.setMaxSpeed(800);
   Y_top_stepper.setAcceleration(2500);
   Y_top_stepper.setInverseRotation(false);