Adding stepper testing stuff to pio

.gitignore

@@ -31,7 +31,6 @@ dist/
 downloads/
 eggs/
 .eggs/
-lib/
 lib64/
 parts/
 sdist/

lib/Messages/src/Messages.hpp

@@ -0,0 +1,29 @@
+#ifndef MESSAGES_HPP
+#define MESSAGES_HPP
+
+#include <Arduino.h>
+
+#include "pb_encode.h"
+#include "pb_decode.h"
+#include "urc.pb.h"
+
+namespace protobuf {
+
+class Messages {
+
+public:
+  static bool decodeRequest(uint8_t *buffer, size_t bufferLen, RequestMessage &requestMessage) {
+    pb_istream_t istream = pb_istream_from_buffer(buffer, bufferLen);
+    return pb_decode(&istream, RequestMessage_fields, &requestMessage);
+  }
+
+  static size_t encodeResponse(uint8_t *buffer, size_t bufferLen, DriveEncodersMessage &driveEncodersMessage) {
+    pb_ostream_t ostream = pb_ostream_from_buffer(buffer, bufferLen);
+    pb_encode(&ostream, DriveEncodersMessage_fields, &driveEncodersMessage);
+    return ostream.bytes_written;
+  }
+};
+
+}  // namespace protobuf
+
+#endif

lib/SoloCAN/src/SoloCAN.cpp

@@ -0,0 +1,105 @@
+#include "SoloCAN.hpp"
+
+namespace solo_can {
+
+SoloCan::SoloCan(FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> &_can) : can(_can) {}
+
+void SoloCan::GetPositionFeedbackCommand(int soloID) {
+    struct CanOpenData data = {
+        .id = (uint16_t)(0x0600 + soloID),
+        .type = SDO_READ_COMMAND,
+        .code = POSITION_FEEDBACK_CODE,
+        .payload = 0
+    };
+    can.write(createMessage(data));
+    delayMicroseconds(200);
+}
+
+void SoloCan::GetSpeedFeedbackCommand(int soloID) {
+    struct CanOpenData data = {
+        .id = (uint16_t)(0x0600 + soloID),
+        .type = SDO_READ_COMMAND,
+        .code = SPEED_FEEDBACK_CODE,
+        .payload = 0
+    };
+    can.write(createMessage(data));
+    delayMicroseconds(200);
+}
+
+void SoloCan::SetSpeedReferenceCommand(int soloID, int speedRef) {
+    uint32_t dir = (uint32_t)(speedRef < 0 ? 1 : 0);
+    uint32_t speedMag = (uint32_t)abs(speedRef);
+    struct CanOpenData data;
+
+    Serial.print("Speed ref: ");
+    Serial.println(speedMag);
+
+    // set speed
+    data = (struct CanOpenData) {
+        .id = (uint16_t)(0x0600 + soloID),
+        .type = SDO_WRITE_COMMAND,
+        .code = SPEED_REF_CODE,
+        .payload = speedMag
+    };
+
+    can.write(createMessage(data));
+    delayMicroseconds(200);
+
+    // set direction
+    data = (struct CanOpenData) {
+        .id = (uint16_t)(0x0600 + soloID),
+        .type = SDO_WRITE_COMMAND,
+        .code = MOTOR_DIRECTION_CODE,
+        .payload = dir
+    };
+
+    can.write(createMessage(data));
+    delayMicroseconds(200);
+}
+
+void SoloCan::GetBoardTemperatureCommand(int soloID) {
+    struct CanOpenData data = (struct CanOpenData){
+      .id = 0x0600 + soloID,
+      .type = SDO_READ_COMMAND,
+      .code = TEMP_CODE,
+      .payload = 0
+    };
+    can.write(createMessage(data));
+    delayMicroseconds(200);
+}
+
+CAN_message_t createMessage(struct CanOpenData data) {
+    CAN_message_t msg;
+    msg.len = 8;
+    msg.id = 0x7FF & data.id;
+    msg.buf[0] = 0x00FF & data.type;
+    msg.buf[1] = 0x00FF & data.code;
+    msg.buf[2] = (0xFF00 & data.code) >> 8;
+    msg.buf[3] = 0;
+    msg.buf[4] = 0x000000FF & data.payload;
+    msg.buf[5] = (0x0000FF00 & data.payload) >> 8;
+    msg.buf[6] = (0x00FF0000 & data.payload) >> 16;
+    msg.buf[7] = (0xFF000000 & data.payload) >> 24;
+
+    return msg;
+}
+
+struct CanOpenData parseMessage(CAN_message_t msg) {
+    struct CanOpenData data;
+    data.id = msg.id;
+    data.type = msg.buf[0];
+    data.code = (msg.buf[2] << 8) | msg.buf[1];
+    data.payload = (msg.buf[7] << 24) | (msg.buf[6] << 16) | (msg.buf[5] << 8) | msg.buf[4];
+
+    return data;
+}
+
+float toFloat(uint32_t payload) {
+    if (payload <=  0x7FFE0000) {
+        return payload / 131072.0;
+    } else {
+        return (0xFFFFFFFF - payload + 0x1) / -131072.0;
+    }
+}
+
+}

lib/SoloCAN/src/SoloCAN.hpp

@@ -0,0 +1,44 @@
+#pragma once
+
+#include <Arduino.h>
+#include <FlexCAN_T4.h>
+
+namespace solo_can {
+
+    const int SDO_READ_COMMAND = 0x40;
+    const int SDO_READ_RESPONSE = 0x42;
+    const int SDO_WRITE_COMMAND = 0x22;
+    const int SDO_WRITE_RESPONSE = 0x60;
+
+    const int TEMP_CODE = 0x3039;
+    const int SPEED_REF_CODE = 0x3005;
+    const int SPEED_FEEDBACK_CODE = 0x3036;
+    const int POSITION_FEEDBACK_CODE = 0x3037;
+    const int MOTOR_DIRECTION_CODE = 0x300C;
+
+    struct CanOpenData {
+        uint16_t id;
+        uint8_t type;
+        uint16_t code;
+        uint32_t payload;
+    };
+
+    CAN_message_t createMessage(struct CanOpenData data);
+    struct CanOpenData parseMessage(CAN_message_t msg);
+
+    float toFloat(uint32_t payload);
+
+    class SoloCan {
+    public:
+        SoloCan(FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> &_can);
+
+        void GetBoardTemperatureCommand(int soloID);
+        void GetSpeedFeedbackCommand(int soloID);
+        void GetPositionFeedbackCommand(int soloID);
+        void SetSpeedReferenceCommand(int soloID, int speedRef);
+
+    private:
+        FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> &can;
+    };
+
+}

platformio.ini

@@ -8,16 +8,47 @@
 ; Please visit documentation for the other options and examples
 ; https://docs.platformio.org/page/projectconf.html
 
-[env:teensy41]
+[platformio]
+default_envs = Stepper
+
+[env]
 platform = teensy
 board = teensy41
 upload_protocol = teensy-cli
-check_tool = cppcheck
 framework = arduino
+
+[env:main]
+check_tool = cppcheck
+build_src_filter = 
+  +<main/*>
 lib_deps = 
   Nanopb
   https://github.com/basicmicro/roboclaw_arduino_library.git
 ;   https://github.com/Solo-FL/SOLO-motor-controllers-ARDUINO-library.git#v3.0
 
+custom_nanopb_protos =
+    +<protos/urc.proto>
+
+[env:MotorTest]
+build_src_filter = 
+  +<MotorTest/*>
+
+[env:ReadTemp]
+build_src_filter = 
+  +<ReadTemp/*>
+
+[env:SoloComputerControl]
+build_src_filter = 
+  +<SoloComputerControl/*>
+lib_deps = 
+  Nanopb
+  QNEthernet
+
+[env:Stepper]
+build_src_filter = 
+  +<Stepper/*>
+lib_deps =
+  https://github.com/janelia-arduino/TMC2209.git
+
 custom_nanopb_protos =
     +<protos/urc.proto>

scripts/ArduinoSketches/ArduinoCanRead/ArduinoCanRead.ino

@@ -0,0 +1,43 @@
+#include <SPI.h>
+#include "mcp_can.h"
+
+#define SPI_CS_PIN  10 
+
+MCP_CAN CAN(SPI_CS_PIN);                                    // Set CS pin
+
+void setup() {
+    Serial.begin(115200);
+    while(!Serial);
+
+    while (CAN_OK != CAN.begin(CAN_1000KBPS)) {
+        Serial.println("CAN BUS FAIL!");
+        delay(100);
+    }
+
+    Serial.println("CAN BUS OK!");
+}
+
+
+void loop() {
+    unsigned char len = 0;
+    unsigned char buf[8];
+
+    if(CAN_MSGAVAIL == CAN.checkReceive()) {
+        CAN.readMsgBuf(&len, buf);
+
+        unsigned long canId = CAN.getCanId();
+
+        Serial.println("-----------------------------");
+        Serial.print("Get data from ID: ");
+        Serial.println(canId, HEX);
+
+        for(int i = 0; i < len; i++) {
+            Serial.print(buf[i], HEX);
+            Serial.print("\t");
+        }
+
+        Serial.println();
+    }
+}
+
+// END FILE

scripts/ArduinoSketches/ArduinoCanWrite/ArduinoCanWrite.ino

@@ -0,0 +1,26 @@
+#include <mcp_can.h>
+#include <SPI.h>
+
+#define SPI_CS_PIN  10 
+
+MCP_CAN CAN(SPI_CS_PIN);
+
+void setup() {
+   Serial.begin(115200);
+   while(!Serial);
+
+    while (CAN_OK != CAN.begin(CAN_500KBPS)) {
+       Serial.println("CAN BUS FAIL!");
+        delay(100);
+    }
+   Serial.println("CAN BUS OK!");
+}
+
+unsigned char stmp[8] = {0, 1, 2, 3, 4, 5, 6, 7};
+
+void loop() {
+    CAN.sendMsgBuf(0x0A, 0, 8, stmp);
+    delay(100);
+}
+
+// END FILE

scripts/ArduinoSketches/ArduinoUnoSoloUnoCanTest/ArduinoUnoSoloUnoCanTest.ino

@@ -0,0 +1,38 @@
+// EXAMPLE of how read the SOLO board temperature, 
+// every second we print the value of the temperature
+
+
+//Importing SOLO Arduino library
+#include "SOLOMotorControllersCanopen.h" 
+
+// Solo Object
+SOLOMotorControllers *SOLO_Obj1; 
+
+// Variables
+float Temperature=0; 
+int fwVersion;
+int error;
+
+// definitions
+const int SOLOdeviceAddress = 0;  // set CAN ID using Motion Studio
+const int chipSelectPin = 10;     // inland CAN shield uses CS pin 10 
+
+void setup() {
+  Serial.begin(115200);
+  SOLO_Obj1 = new SOLOMotorControllersCanopen(SOLOdeviceAddress, chipSelectPin);  
+}
+
+void loop() {
+  //Reading
+  Temperature = SOLO_Obj1->GetBoardTemperature(error);
+  fwVersion = SOLO_Obj1->GetDeviceFirmwareVersion(error);
+
+  //Print
+  Serial.println("Read from SOLO");
+  Serial.println(Temperature,7);
+  Serial.println(fwVersion, HEX);
+  Serial.println("Error");
+  Serial.println(error);
+
+  delay(1000);
+}