added sizeof based claculation of msg size, added a hack to be able to read the RX...

diff --git a/src/modules/tools/spindle/HuanyangSpindleControl.cpp b/src/modules/tools/spindle/HuanyangSpindleControl.cpp
index e8dea31..e7650ef 100644 (file)
--- a/src/modules/tools/spindle/HuanyangSpindleControl.cpp
+++ b/src/modules/tools/spindle/HuanyangSpindleControl.cpp
@@ -5,6 +5,8 @@
       You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
 */
 
+#include "libs/Kernel.h"
+#include "StreamOutputPool.h"
 #include "BufferedSoftSerial.h"
 #include "ModbusSpindleControl.h"
 #include "HuanyangSpindleControl.h"
@@ -16,7 +18,7 @@ void HuanyangSpindleControl::turn_on()
     // prepare data for the spindle off command
     char turn_on_msg[6] = { 0x01, 0x03, 0x01, 0x01, 0x00, 0x00 };
     // calculate CRC16 checksum
-    unsigned int crc = modbus->crc16(turn_on_msg, 4);
+    unsigned int crc = modbus->crc16(turn_on_msg, sizeof(turn_on_msg)-2);
     turn_on_msg[4] = crc & 0xFF;
     turn_on_msg[5] = (crc >> 8);
 
@@ -24,9 +26,9 @@ void HuanyangSpindleControl::turn_on()
     modbus->dir_output->set();
     modbus->delay(1);
     // send the actual message
-    modbus->serial->write(turn_on_msg, 6);
+    modbus->serial->write(turn_on_msg, sizeof(turn_on_msg));
     // wait a calculated time for the data to be sent 
-    modbus->delay((int) ceil(6 * modbus->delay_time));
+    modbus->delay((int) ceil(sizeof(turn_on_msg) * modbus->delay_time));
     // disable transmitter
     modbus->dir_output->clear();
     // wait 50ms, required by the Modbus standard
@@ -40,7 +42,7 @@ void HuanyangSpindleControl::turn_off()
     // prepare data for the spindle off command
     char turn_off_msg[6] = { 0x01, 0x03, 0x01, 0x08, 0x00, 0x00 };
     // calculate CRC16 checksum
-    unsigned int crc = modbus->crc16(turn_off_msg, 4);
+    unsigned int crc = modbus->crc16(turn_off_msg, sizeof(turn_off_msg)-2);
     turn_off_msg[4] = crc & 0xFF;
     turn_off_msg[5] = (crc >> 8);
 
@@ -48,9 +50,9 @@ void HuanyangSpindleControl::turn_off()
     modbus->dir_output->set();
     modbus->delay(1);
     // send the actual message
-    modbus->serial->write(turn_off_msg, 6);
+    modbus->serial->write(turn_off_msg, sizeof(turn_off_msg));
     // wait a calculated time for the data to be sent 
-    modbus->delay((int) ceil(6 * modbus->delay_time));
+    modbus->delay((int) ceil(sizeof(turn_off_msg) * modbus->delay_time));
     // disable transmitter
     modbus->dir_output->clear();
     // wait 50ms, required by the Modbus standard
@@ -89,7 +91,43 @@ void HuanyangSpindleControl::set_speed(int target_rpm)
 
 void HuanyangSpindleControl::report_speed() 
 {
+    // clear RX buffer before start
+    while(modbus->serial->readable()){
+        modbus->serial->getc();
+    }
 
-    // TODO: implement this
+    // prepare data for the get speed command
+    char get_speed_msg[8] = { 0x01, 0x04, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00 };
+    // calculate CRC16 checksum
+    unsigned int crc = modbus->crc16(get_speed_msg, sizeof(get_speed_msg)-2);
+    get_speed_msg[6] = crc & 0xFF;
+    get_speed_msg[7] = (crc >> 8);
+
+    // enable transmitter
+    modbus->dir_output->set();
+    modbus->delay(1);
+    // send the actual message
+    modbus->serial->write(get_speed_msg, sizeof(get_speed_msg));
+    // wait a calculated time for the data to be sent 
+    modbus->delay((int) ceil(sizeof(get_speed_msg) * modbus->delay_time));
+    // disable transmitter
+    modbus->dir_output->clear();
+    // wait 50ms, required by the Modbus standard
+    modbus->delay(50);
+
+    // wait for the complete message to be received
+    modbus->delay((int) ceil(8 * modbus->delay_time));
+    // prepare an array for the answer
+    char speed[8] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+   
+    // read the answer into the buffer
+    for(int i=0; i<8; i++) {
+        speed[i] = modbus->serial->getc();
+    }
+    // get the Hz value from trhe answer and convert it into an RPM value
+    unsigned int hz = (speed[4] << 8) | speed[5];
+    unsigned int rpm = hz / 100 * 60;
 
+    // report the current RPM value
+    THEKERNEL->streams->printf("Current RPM: %d\n", rpm);
 }