Inital rewrite of stepticker and associated code to implement accleration per tick.

[clinton/Smoothieware.git] / src / libs / Kernel.cpp

diff --git a/src/libs/Kernel.cpp b/src/libs/Kernel.cpp
index 0ad866b..6dcffae 100644 (file)
--- a/src/libs/Kernel.cpp
+++ b/src/libs/Kernel.cpp
@@ -109,7 +109,7 @@ Kernel::Kernel(){
     add_module( this->slow_ticker = new SlowTicker());
 
     this->step_ticker = new StepTicker();
-    this->adc = new Adc();
+    this->adc = new(AHB0) Adc();
 
     // TODO : These should go into platform-specific files
     // LPC17xx-specific
@@ -118,7 +118,6 @@ Kernel::Kernel(){
     NVIC_SetPriority(TIMER1_IRQn, 1);
     NVIC_SetPriority(TIMER2_IRQn, 4);
     NVIC_SetPriority(PendSV_IRQn, 3);
-    NVIC_SetPriority(RIT_IRQn, 3); // we make acceleration tick the same prio as pendsv so it can't be pre-empted by end of block
 
     // Set other priorities lower than the timers
     NVIC_SetPriority(ADC_IRQn, 5);
@@ -143,9 +142,8 @@ Kernel::Kernel(){
     this->acceleration_ticks_per_second = THEKERNEL->config->value(acceleration_ticks_per_second_checksum)->by_default(1000)->as_number();
 
     // Configure the step ticker ( TODO : shouldnt this go into stepticker's code ? )
-    this->step_ticker->set_reset_delay( microseconds_per_step_pulse );
+    this->step_ticker->set_unstep_time( microseconds_per_step_pulse );
     this->step_ticker->set_frequency( this->base_stepping_frequency );
-    this->step_ticker->set_acceleration_ticks_per_second(acceleration_ticks_per_second); // must be set after set_frequency
 
     // Core modules
     this->add_module( this->gcode_dispatch = new GcodeDispatch() );
@@ -154,7 +152,7 @@ Kernel::Kernel(){
     this->add_module( this->conveyor       = new Conveyor()      );
     this->add_module( this->simpleshell    = new SimpleShell()   );
 
-    this->planner = new Planner();
+    this->planner = new(AHB0) Planner();
     this->configurator   = new Configurator();
 }
 
@@ -165,6 +163,7 @@ std::string Kernel::get_query_string()
     bool homing;
     bool ok = PublicData::get_value(endstops_checksum, get_homing_status_checksum, 0, &homing);
     if(!ok) homing= false;
+    bool running= false;
 
     str.append("<");
     if(halted) {
@@ -176,30 +175,48 @@ std::string Kernel::get_query_string()
     }else if(this->conveyor->is_queue_empty()) {
         str.append("Idle,");
     }else{
+        running= true;
         str.append("Run,");
     }
 
-    // get real time current actuator position in mm
-    ActuatorCoordinates current_position{
-        robot->actuators[X_AXIS]->get_current_position(),
-        robot->actuators[Y_AXIS]->get_current_position(),
-        robot->actuators[Z_AXIS]->get_current_position()
-    };
-
-    // get machine position from the actuator position using FK
-    float mpos[3];
-    robot->arm_solution->actuator_to_cartesian(current_position, mpos);
-
-    char buf[64];
-    // machine position
-    size_t n= snprintf(buf, sizeof(buf), "%f,%f,%f,", mpos[0], mpos[1], mpos[2]);
-    str.append("MPos:").append(buf, n);
-
-    // work space position
-    Robot::wcs_t pos= robot->mcs2wcs(mpos);
-    n= snprintf(buf, sizeof(buf), "%f,%f,%f", robot->from_millimeters(std::get<X_AXIS>(pos)), robot->from_millimeters(std::get<Y_AXIS>(pos)), robot->from_millimeters(std::get<Z_AXIS>(pos)));
-    str.append("WPos:").append(buf, n);
-    str.append(">\r\n");
+    if(running) {
+        // get real time current actuator position in mm
+        ActuatorCoordinates current_position{
+            robot->actuators[X_AXIS]->get_current_position(),
+            robot->actuators[Y_AXIS]->get_current_position(),
+            robot->actuators[Z_AXIS]->get_current_position()
+        };
+
+        // get machine position from the actuator position using FK
+        float mpos[3];
+        robot->arm_solution->actuator_to_cartesian(current_position, mpos);
+
+        char buf[128];
+        // machine position
+        size_t n= snprintf(buf, sizeof(buf), "%1.4f,%1.4f,%1.4f,", robot->from_millimeters(mpos[0]), robot->from_millimeters(mpos[1]), robot->from_millimeters(mpos[2]));
+        str.append("MPos:").append(buf, n);
+
+        // work space position
+        Robot::wcs_t pos= robot->mcs2wcs(mpos);
+        n= snprintf(buf, sizeof(buf), "%1.4f,%1.4f,%1.4f", robot->from_millimeters(std::get<X_AXIS>(pos)), robot->from_millimeters(std::get<Y_AXIS>(pos)), robot->from_millimeters(std::get<Z_AXIS>(pos)));
+        str.append("WPos:").append(buf, n);
+        str.append(">\r\n");
+
+    }else{
+        // return the last milestone if idle
+        char buf[128];
+        // machine position
+        Robot::wcs_t mpos= robot->get_axis_position();
+        size_t n= snprintf(buf, sizeof(buf), "%1.4f,%1.4f,%1.4f,", robot->from_millimeters(std::get<X_AXIS>(mpos)), robot->from_millimeters(std::get<Y_AXIS>(mpos)), robot->from_millimeters(std::get<Z_AXIS>(mpos)));
+        str.append("MPos:").append(buf, n);
+
+        // work space position
+        Robot::wcs_t pos= robot->mcs2wcs(mpos);
+        n= snprintf(buf, sizeof(buf), "%1.4f,%1.4f,%1.4f", robot->from_millimeters(std::get<X_AXIS>(pos)), robot->from_millimeters(std::get<Y_AXIS>(pos)), robot->from_millimeters(std::get<Z_AXIS>(pos)));
+        str.append("WPos:").append(buf, n);
+        str.append(">\r\n");
+
+    }
     return str;
 }