Improve library usage and example

Author: facchinm

examples/Test/Test.ino

@@ -1,74 +1,51 @@
 #include "MKRMotorShield.h"
 
-const int pingPin = IN4;
-const byte interruptPin = IN2;
-
-long encoderResetValue = 5;
-
-mc::MotorController controller;
-
-mc::ServoMotor servo1 = mc::ServoMotor();
-mc::ServoMotor servo2 = mc::ServoMotor();
-mc::ServoMotor servo3 = mc::ServoMotor();
-mc::ServoMotor servo4 = mc::ServoMotor();
-
-mc::DCMotor M2 = mc::DCMotor();
-mc::DCMotor M1 = mc::DCMotor();
-d21::DCMotor M3 = d21::DCMotor();
-d21::DCMotor M4 = d21::DCMotor();
-
-mc::Encoder encoder2 = mc::Encoder();
-mc::Encoder encoder1 = mc::Encoder();
-
-mc::PID pid2 = mc::PID();
-mc::PID pid1 = mc::PID();
-
-volatile uint8_t irq_status;
-
-void getDataIrq() {
-  irq_status = 1;
-};
-
-int m2_pos = 0;
-//boolean toggle1 = false;
-//boolean toggle2 = false;
-
 void setup() {
-  
+
   Serial.begin(115200);
   while (!Serial);
-  // create mkrshield connection
-  Wire.begin();
-  
-  delay(500);
 
-  //create encoder2 object in MATLAB
-  pinMode(IRQ_PIN, INPUT_PULLUP);
-  attachInterrupt(IRQ_PIN, getDataIrq, FALLING);  
-  encoder2.resetCounter(0); 
-  
-// create dcmotor2 object in matlab
-  M2.setFrequency(100);  
+  // Start communicationg with the motor shield
+  if (controller.begin()) {
+    Serial.print("MKR Motor Shield connected, firmware version ");
+    Serial.println(controller.getFWVersion());
+  } else {
+    Serial.println("Couldn't connect! Is the red led blinking? You may need to update the firmware with FWUpdater sketch");
+    while (1);
+  }
+
+  // Reboot the motor controller; brings every value back to default
+  controller.reboot();
 
-  // enable dc motor2 object
+  // Reset the encoder internal counter to zero (can be set to any initial value)
+  encoder1.resetCounter(0);
 
-  M2.setDuty(90);
+  // Start DC Motor 1 (labeled M1) with 90% of maximum speed, clockwise
+  M1.setFrequency(100);
+  M1.setDuty(90);
 
-  while(m2_pos < 36000){
-    m2_pos = encoder2.getRawCount();
+  // Read the encoder connected to Motor1 until it reaches 36000 counts
+  int motor1Pos = 0;
+  while (motor1Pos < 36000) {
+    motor1Pos = encoder1.getRawCount();
+    // Remember to call ping() here and there!
+    // If you forget to do so the controller will think that the user sketch is not running and will freeze.
+    // You can wait up to 3 seconds between pings before the reset kicks in
     controller.ping();
   }
-  Serial.println(m2_pos);
-  //M2.setDuty(0);   
-  pid2.setControlMode(CL_POSITION);
-  pid2.setGains(25, 0, 3);
-  pid2.setMaxAcceleration(4000); 
-  controller.ping();
-  delay(1000);
-  pid2.setSetpoint(TARGET_POSITION, 10000);
+
+  // Switch motor control from DIRECT to PID-driven.
+  // This way, you can program the motor to reach a certain position or velocity without any further intervention.
+  // The PID can be carefully tuned if a particular profile is needed.
+  pid1.setControlMode(CL_POSITION);
+  pid1.setGains(25, 0, 3);
+  pid1.setMaxAcceleration(4000);
+  pid1.setSetpoint(TARGET_POSITION, 5000);
 }
+
 void loop() {
-  m2_pos = encoder2.getRawCount();
-  Serial.println(m2_pos);
+  // Simply print the actual position while the PID is running, pinging the controller every loop()
+  Serial.println(encoder1.getRawCount());
   controller.ping();
+  delay(100);
 }

src/Common.h

@@ -44,7 +44,9 @@ enum IRQCause {
 #define IN3 A5
 #define IN4 A2
 
+namespace mc {
 int getData(Commands cmd, uint8_t target, uint8_t* buf);
 void setData(Commands cmd, uint8_t target, int data);
 void setDataPIDGains(Commands cmd, uint8_t target, int16_t P, int16_t I, int16_t D);
 int getData(Commands cmd, uint8_t* buf);
+}

src/MKRMotorShield.cpp

@@ -1,5 +1,6 @@
 #include "MKRMotorShield.h"
 
+namespace mc {
 void setDataPIDGains(Commands cmd, uint8_t target, int16_t P, int16_t I, int16_t D) {
   Wire.beginTransmission(I2C_ADDRESS);
   Wire.write((uint8_t)cmd);
@@ -37,3 +38,25 @@ int getData(Commands cmd, uint8_t target, uint8_t* buf) {
 int getData(Commands cmd, uint8_t* buf) {
   return getData(cmd, 0, buf);
 }
+}
+
+mc::MotorController controller;
+
+mc::ServoMotor servo1;
+mc::ServoMotor servo2;
+mc::ServoMotor servo3;
+mc::ServoMotor servo4;
+
+mc::DCMotor M1;
+mc::DCMotor M2;
+d21::DCMotor M3;
+d21::DCMotor M4;
+
+mc::Encoder encoder1;
+mc::Encoder encoder2;
+
+mc::PID pid1;
+mc::PID pid2;
+
+mc::Battery battery;
+

src/MKRMotorShield.h

@@ -7,8 +7,22 @@
 #include "Common.h"
 #include "PID.h"
 
-void setDataPIDGains(Commands cmd, uint8_t target, int16_t P, int16_t I, int16_t D);
-void setData(Commands cmd, uint8_t target, int data);
-int getData(Commands cmd, uint8_t target, uint8_t* buf);
-int getData(Commands cmd, uint8_t* buf);
+extern mc::MotorController controller;
 
+extern mc::ServoMotor servo1;
+extern mc::ServoMotor servo2;
+extern mc::ServoMotor servo3;
+extern mc::ServoMotor servo4;
+
+extern mc::DCMotor M1;
+extern mc::DCMotor M2;
+extern d21::DCMotor M3;
+extern d21::DCMotor M4;
+
+extern mc::Encoder encoder1;
+extern mc::Encoder encoder2;
+
+extern mc::PID pid1;
+extern mc::PID pid2;
+
+extern mc::Battery battery;

src/MotorController.cpp

@@ -1,5 +1,6 @@
 #include "MotorController.h"
 #include "Common.h"
+#include "Wire.h"
 
 String mc::MotorController::getFWVersion() {
   char ret[5];
@@ -10,6 +11,7 @@ String mc::MotorController::getFWVersion() {
 
 void mc::MotorController::reboot() {
   setData(RESET, 0, 0);
+  delay(500);
 }
 
 void mc::MotorController::ping() {
@@ -21,3 +23,12 @@ float mc::MotorController::getTemperature() {
   getData(GET_INTERNAL_TEMP, (uint8_t*)&ret);
   return (float)ret / 1000.0f;
 }
+
+int mc::MotorController::begin() {
+  Wire.begin();
+  String version = getFWVersion();
+  if (version.c_str()[0] == '0') {
+    return 1;
+  }
+  return 0;
+};

src/MotorController.h

@@ -10,6 +10,7 @@ class MotorController {
     String getFWVersion();
     void reboot();
     void ping();
+    int begin();
     float getTemperature();
 };
 }