pill and barrel commands

homing and pill dispensing to be reworked

HTequi-firmware/src/Atm_lien/Atm_TeensyStep.cpp

@@ -131,7 +131,7 @@ Atm_TeensyStep& Atm_TeensyStep::moveTo(long int stepAbs ) {
 }
 
 Atm_TeensyStep& Atm_TeensyStep::stop(){
-  controller->stopAsync();
+  controller->emergencyStop();
   //trigger( EVT_STOPPING );
 }
 /*

HTequi-firmware/src/blobcnc_feeder/main.cpp

@@ -27,11 +27,18 @@ EthernetUDP Udp;
 
 
 /////////////////// STEPPER machines  ////////////////////////
-const int HIGH_SPEED = 1000 ;
+const int HIGH_SPEED = 8000 ;
 const int HIGH_ACC = 8000 ;
 const int LOW_SPEED = 100 ;
-const int LOW_ACC = 5000 ;
-const int BARREL_DISTANCE = 8592 ;
+const int LOW_ACC = 1000 ;
+const int BARREL_DISTANCE = 10 ; //in full steps
+
+const int REDUCTION_RATIO = 26.85;
+
+const int PILL_MICROSTEP = 32;
+const int BARREL_MICROSTEP = 32;
+
+const int BARREL_THRESHOLD = 0 ;
 
 Atm_TeensyStep barrel_step;
 Stepper barrel_stepper(3, 2);
@@ -43,37 +50,71 @@ Atm_TeensyStep pill_step;
 Stepper pill_stepper(6, 5);
 StepControl pill_controller ;
 
+Atm_analog pill_sensor ;
+
 
 void barrel_homing(){
   digitalWrite( 13, HIGH );
   barrel_stepper.setMaxSpeed(LOW_SPEED);
   barrel_stepper.setAcceleration(LOW_ACC);
-  barrel_step.move(BARREL_DISTANCE);
+  barrel_step.move(BARREL_DISTANCE*BARREL_MICROSTEP*REDUCTION_RATIO);
   Serial.println(barrel_sensor.state());
-  while(barrel_sensor.state()<500 && (barrel_step.state() == barrel_step.RUNNING)){ // &&
-    //automaton.run();
+  // while(barrel_sensor.state()>900 && (barrel_step.state() == barrel_step.RUNNING)){ // &&
+  //
+  //   //barrel_step.cycle();
+  //   //Serial.println(barrel_sensor.state());
+  // }
+  // barrel_step.stop();
+  // barrel_stepper.setPosition(0);
+  bool foundHome = false ;
+  while(barrel_step.state() != barrel_step.STOPPING){
+    // automaton.run();
     barrel_step.cycle();
-    Serial.println(barrel_sensor.state());
+    if(barrel_sensor.state()<BARREL_THRESHOLD){
+      barrel_step.stop();
+      barrel_stepper.setPosition(0);
+      foundHome = true ;
+      break ;
+    }
   }
-  barrel_step.stop();
-  barrel_stepper.setPosition(0);
   digitalWrite( 13, LOW );
+  if (!foundHome){Serial.println("HOMING FAILED - CHECK SENSOR");}
+  else{Serial.println("homing done");}
 
 }
 
-void barrel_next(){
-  digitalWrite( 13, HIGH );
+
+void barrel_move(int stepRel){
   barrel_stepper.setMaxSpeed(HIGH_SPEED);
   barrel_stepper.setAcceleration(HIGH_ACC);
-  barrel_step.move(BARREL_DISTANCE);
+  barrel_step.move(stepRel);
+}
+
+void pill_move(int stepRel){
+  pill_stepper.setMaxSpeed(HIGH_SPEED);
+  pill_stepper.setAcceleration(HIGH_ACC);
+  pill_step.move(stepRel);
+}
+
+void pill_next(){
+  pill_stepper.setMaxSpeed(HIGH_SPEED);
+  pill_stepper.setAcceleration(HIGH_ACC);
+
+  //make one turn, if nothing was seen by sensor try again up to 5 times
+
+  // for (int retries = 0 ; retries < 5; retries
+  // pill_step.move(200*PILL_MICROSTEP);
+
 }
 
 //////////////  Serial command machine  /////////////////////
+
 char cmd_buffer[80];
 Atm_command cmd;
-enum { CMD_TEST, CMD_BARREL_MOVE, CMD_BARREL_HOME, CMD_BARREL_NEXT };
+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";
+    "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);
@@ -84,20 +125,26 @@ void cmd_callback( int idx, int v, int up ) {
       Serial.println("in test");
       return;
     case CMD_BARREL_MOVE:
-      digitalWrite( 13, LOW );
-      barrel_step.move(atoi( cmd.arg( 1 ) ));
+      barrel_move(atoi( cmd.arg( 1 ) ));
       return;
     case CMD_BARREL_HOME:
       barrel_homing();
       return;
     case CMD_BARREL_NEXT:
-      barrel_next();
+      barrel_move(BARREL_DISTANCE);
+      if(!barrel_sensor.state()>BARREL_THRESHOLD){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)
@@ -130,6 +177,7 @@ void setup() {
   pill_step.begin(pill_stepper, pill_controller);
   pill_stepper.setMaxSpeed(HIGH_SPEED);
   pill_stepper.setAcceleration(HIGH_ACC);
+  pill_sensor.begin(A3);
   //stepper.onOnchange(Machine &machine, optional int event = 0)
   //stepper.cycle(1000);
   //barrel_step.gotoStep(10000);
@@ -137,6 +185,8 @@ void setup() {
 
 }
 
+
+
 void loop() {
   automaton.run();