Actuator Embedded Code and Tests #184

Actuator testing

Author: JPGC04

pio_workspace/src/actuator_main.cpp

@@ -2,12 +2,81 @@
 
 #include "actuator_main.h"
 
-void actuator_setup(){
-    // put your setup code here, to run once:
+#include <Arduino.h>
+
+#include <Servo.h>
+
+#include <ros.h>
+#include <std_msgs/UInt16.h>
+#include <std_msgs/Bool.h>
+
+// servo pin definition
+const int SERVO_PIN = 9;
+
+// create node handle
+ros::NodeHandle nh;
+
+// create grabber servo object
+Servo grabberServo;
+
+// function prototypes
+void servoPosition_CB(const std_msgs::UInt16& msg);
+void servoSweep_CB(const std_msgs::Bool& msg);
+
+// subscribe to topics
+ros::Subscriber<std_msgs::UInt16> grabberPositionSub("servo/position", &servoPosition_CB);
+ros::Subscriber<std_msgs::Bool> grabberSweepSub("servo/sweep", &servoSweep_CB);
+
+// setup function
+void actuator_setup() {
+    // pwm teensy pin connected to grabber
+  pinMode(SERVO_PIN, OUTPUT);
+    // attach servo object to teensy pin and check for success
+  if (grabberServo.attach(SERVO_PIN)) {
+    // servo attached successfully
+  } else {
+    // servo attachment failed - could add LED blink or error handling here
+  }
+    // initialize ros node
+  nh.initNode();
+    // subscribe to ros topics
+  nh.subscribe(grabberPositionSub);
+  nh.subscribe(grabberSweepSub);
+}
+
+// check for new messages with 1ms delay
+void actuator_loop() {
+  nh.spinOnce();
+  delay(1);
+}
+
+// move grabber servo to position specified by ros message
+void servoPosition_CB(const std_msgs::UInt16& msg) {
+  // constrain servo position to valid range (0-180 degrees)
+  int position = constrain(msg.data, 0, 180);
+  grabberServo.write(position);
 }
 
-void actuator_loop(){
-    // put your main code here, to run repeatedly:
+// sweep servo based on boolean message
+void servoSweep_CB(const std_msgs::Bool& msg) {
+  if (msg.data) {
+    // sweep from 0 to 180 degrees
+    for (int pos = 0; pos <= 180; pos += 1) {
+      grabberServo.write(pos);
+      delay(15);
+      // check for new messages during sweep to maintain some responsiveness
+      nh.spinOnce();
+    }
+  }
+  else {
+    // sweep from 180 to 0 degrees
+    for (int pos = 180; pos >= 0; pos -= 1) {
+      grabberServo.write(pos);
+      delay(15);
+      // check for new messages during sweep to maintain some responsiveness
+      nh.spinOnce();
+    }
+  }
 }
 
 #endif

pio_workspace/test/actuator_board/actuator_maybe_current_test.cpp

@@ -0,0 +1,105 @@
+
+// PRINTS CURRENT READINGS FROM MOTOR RUNNING OFF J3
+
+#include <Servo.h>
+#define CURRENT_PIN 17 //CHANGED FROM 16
+#define SERVO_PIN 10
+#define GREEN_LED 15
+#define RED_LED 14
+
+
+// variables
+int contact_current = 400;
+int max_position = 180;
+int pos = 0;    // variable to store the servo position 
+float current = 0;
+
+
+
+// create servo object
+Servo servo1;  // create servo object to control a servo 
+                // a maximum of eight servo objects can be created 
+ 
+
+unsigned long previousMillis = 0; // stores last time the servo was updated
+unsigned long previousServoMillis = 0; // stores last time the servo was updated
+const long interval = 1000; // interval at which to switch direction (milliseconds)
+const long servoInterval = 2;
+
+bool direction = false;
+
+void setup() 
+{ 
+
+  // set data rate
+  Serial.begin(9600); 
+
+  // set i/o pins
+  pinMode(CURRENT_PIN, INPUT); // pin16 is IC input
+  pinMode(LED_BUILTIN, OUTPUT); // builtin LED on teesny
+  servo1.attach(SERVO_PIN); // servo to control grabber
+} 
+ 
+
+void loop()
+{
+
+  close_actuator();
+
+}
+
+void close_actuator() {
+  for (pos = 0; pos <= max_position; pos += 1) { // position maximum should be set to whatever would see grabber fully closed
+    
+    // check current reading
+    int currentReading = analogRead(CURRENT_PIN);
+    Serial.println(currentReading);
+
+    // use current reading to check if made contact
+    if(currentReading > contact_current){
+      
+      stop();
+
+      // STOP THE SERVO
+    }
+    else{ // servo keeps going
+      
+      digitalWrite(GREEN_LED, HIGH);
+    }
+    
+    // move servo
+    servo1.write(pos);              // tell servo to go to position in variable 'pos'
+    delay(15);                       // waits 15ms for the servo to reach the position
+  }
+
+}
+
+void open_actuator() {
+  for (pos = max_position; pos >= 0; pos -= 1) { // position maximum should be set to whatever would see grabber fully closed
+    
+    // check current reading
+    int currentReading = analogRead(CURRENT_PIN);
+    Serial.println(currentReading);
+
+    // use current reading to check if made contact
+    if(currentReading > contact_current){
+    
+      digitalWrite(LED_BUILTIN, HIGH);
+    }
+    else{ // servo keeps going
+  
+      digitalWrite(LED_BUILTIN, LOW);
+    }
+    
+    // ros thingie to make happen
+    // move servo
+    servo1.write(pos);              // tell servo to go to position in variable 'pos'
+    delay(15);                       // waits 15ms for the servo to reach the position
+  }}
+
+  void stop() {
+    for (;;) {
+      digitalWrite(RED_LED, HIGH);
+
+    }
+  }

pio_workspace/test/actuator_board/actuator_motor_test/actuator_motor_test.ino

@@ -0,0 +1,28 @@
+// USING SERVO CONNECTED TO ACTUATOR BOARD
+
+#include <Servo.h>
+
+Servo myservo;  // create Servo object to control a servo
+// twelve Servo objects can be created on most boards
+
+int pos = 0;    // variable to store the servo position
+
+void setup() {
+  myservo.attach(11);  // attaches the servo on pin x to the Servo object
+  Serial.begin(115200); // connection speed (req field for actuator board)
+}
+
+
+void loop() {
+  for (pos = 0; pos <= 100; pos += 1) { // goes from 0 degrees to x degrees
+    // in steps of 1 degree
+    Serial.println("working");
+    myservo.write(pos);              // tell servo to go to position in variable 'pos'
+    delay(15);                       // waits 15 ms for the servo to reach the position
+  }
+ 
+  //for (pos = 300; pos >= 0; pos -= 1) { // goes from x degrees to 0 degrees
+   // myservo.write(pos);              // tell servo to go to position in variable 'pos '
+   // delay(15);                       // waits 15 ms for the servo to reach the position
+ // }
+}

pio_workspace/test/actuator_board/current_sense_test/current_sense_test.ino

@@ -0,0 +1,86 @@
+
+// PRINTS CURRENT READINGS FROM MOTOR RUNNING OFF J3
+
+#include <Servo.h>
+#define CURRENT_PIN 17 //CHANGED FROM 16
+#define SERVO_PIN 10
+#define GREEN_LED 15
+#define RED_LED 14
+
+
+// variables
+int contact_current = 400;
+int max_position = 180;
+int pos = 0;    // variable to store the servo position 
+float current = 0;
+
+
+
+// create servo object
+Servo servo1;  // create servo object to control a servo 
+                // a maximum of eight servo objects can be created 
+ 
+
+unsigned long previousMillis = 0; // stores last time the servo was updated
+unsigned long previousServoMillis = 0; // stores last time the servo was updated
+const long interval = 1000; // interval at which to switch direction (milliseconds)
+const long servoInterval = 2;
+
+bool direction = false;
+
+void setup() 
+{ 
+
+  // set data rate
+  Serial.begin(9600); 
+
+  // set i/o pins
+  pinMode(CURRENT_PIN, INPUT); // pin16 is IC input
+  pinMode(LED_BUILTIN, OUTPUT); // builtin LED on teesny
+  servo1.attach(SERVO_PIN); // servo to control grabber
+} 
+ 
+
+void loop()
+{
+
+  close_actuator();
+
+}
+
+void close_actuator() {
+  for (pos = 0; pos <= max_position; pos += 1) { // position maximum should be set to whatever would see grabber fully closed
+    
+    // check current reading
+    int currentReading = analogRead(CURRENT_PIN);
+    Serial.println(currentReading);
+
+    // use current reading to check if made contact
+    if(currentReading > contact_current){
+      
+      stop();
+
+      // STOP THE SERVO
+    }
+    else{ // servo keeps going
+      
+      digitalWrite(GREEN_LED, HIGH);
+    }
+    
+    // move servo
+    servo1.write(pos);              // tell servo to go to position in variable 'pos'
+    delay(15);                       // waits 15ms for the servo to reach the position
+    pos = pos;
+  }
+
+}
+
+
+
+
+  void stop() {
+    for (;;) {
+      digitalWrite(RED_LED, HIGH);
+      
+    }
+  }