add servo functions

prosthesis_controller.ino

@@ -11,6 +11,8 @@ void setup() {
 
 
   pinMode(PIN_INPUTARM1,INPUT);
+
+  initServos();
 }
 
 
@@ -27,14 +29,19 @@ void loop() {
 
   loopVacuum(loopmillis);
 
+  loopServos(loopmillis);
+
 
   //Print Debug Information
   static unsigned long last_print=0;
   if (loopmillis - last_print >100) {
     last_print=loopmillis;
-    printVacuumValues();
+    //printVacuumValues();
+    //Serial.print(" Vac="); Serial.print(vac);
+
+    printServoDebug();
+    
     
-    Serial.print(" Vac="); Serial.print(vac);
     Serial.println();
     Serial.println();
   }

servo.cpp

@@ -1,2 +1,221 @@
 #include "Arduino.h"
-#include "servo.h"
+#include "servo.h"
+
+using namespace DYNAMIXEL;
+
+#define SERVO_COUNT 3
+const uint8_t DXL_ID = 1;
+const float DXL_PROTOCOL_VERSION = 2.0;
+
+dxl_servo servos[SERVO_COUNT];
+
+Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
+
+void initServos()
+{
+  //Main Arm first Servo. Model 1020
+  servos[0].id=11;
+  servos[0].mode=OP_CURRENT_BASED_POSITION;
+  servos[0].current=10.0;
+  servos[0].offset_angle=180;
+  servos[0].orientation=1;
+  servos[0].modelnumber=1020;
+
+  //Main Arm second Servo. Model 1080
+  servos[1].id=12;
+  servos[1].mode=OP_POSITION;
+  servos[1].offset_angle=180;
+  servos[1].orientation=1;
+  servos[1].modelnumber=1080;
+
+  //Main Arm third Servo. Model 1080
+  servos[2].id=13;
+  servos[2].mode=OP_POSITION;
+  servos[2].offset_angle=180;
+  servos[2].orientation=1;
+  servos[2].modelnumber=1080;
+
+  
+  for(uint8_t j=0;j<10;j++) { //Startup Wait
+    Serial.print(j+1);Serial.println("/10");
+    delay(200);
+  }
+  /*changeID(1,21);
+  scanDynamixel(57600,DXL_PROTOCOL_VERSION,64);
+  
+  while(1){
+    delay(100);
+  }*/
+
+  dxl.begin(57600);
+  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
+  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
+
+  uint8_t servoErrors=0;
+  for (uint8_t i=0;i<SERVO_COUNT;i++) {
+    
+    // Get DYNAMIXEL information
+    if (!dxl.ping(servos[i].id)){
+      Serial.print("Warning: Servo "); Serial.print(i); Serial.print(" does not respond ");
+      Serial.println();
+      delay(2000);
+      servoErrors++;
+    }else{
+      uint16_t servomodel=dxl.getModelNumber(servos[i].id);
+      if (servos[i].modelnumber!=0 && servomodel!=servos[i].modelnumber) { //if modelnumber given, check if it matches
+        Serial.print("Warning: Servo "); Serial.print(i); Serial.print(" with id "); Serial.print(servos[i].id); Serial.print(" with Model "); Serial.print(servomodel); Serial.print(" does not match "); Serial.print(servos[i].modelnumber);
+        Serial.println();
+        servoErrors++;
+        delay(2000);
+      }
+
+      // Turn off torque when configuring items in EEPROM area
+      dxl.torqueOff(servos[i].id);
+
+      dxl.setOperatingMode(servos[i].id, servos[i].mode);
+      if (servos[i].current>0 && servos[i].mode==OP_CURRENT_BASED_POSITION) {
+        dxl.setGoalCurrent(servos[i].id, servos[i].current, UNIT_PERCENT);
+      }
+
+      dxl.writeControlTableItem(ControlTableItem::PROFILE_VELOCITY, servos[i].id, 30); //was 30
+      dxl.writeControlTableItem(ControlTableItem::PROFILE_ACCELERATION, servos[i].id, 10);
+      
+      dxl.torqueOn(servos[i].id);
+    }
+
+    
+
+    
+  }
+
+  
+  if (servoErrors>0) {
+    scanDynamixel(57600,DXL_PROTOCOL_VERSION,DXL_BROADCAST_ID);
+    while(1){
+      delay(100);
+    }
+  }
+  
+}
+
+
+void loopServos(unsigned long millis) {
+  servos[0].angle=sin(millis/1000.0/1.0)*20;
+  servos[1].angle=cos(millis/1000.0/1.0)*20;
+  servos[2].angle=sin(millis/1000.0/1.0)*20;
+
+  static unsigned long last_servo_update=0;
+  if (millis-last_servo_update>SERVO_UPDATE_INTERVAL){
+    last_servo_update=millis;
+    for (uint8_t i=0;i<SERVO_COUNT;i++) {
+      dxl.setGoalPosition(servos[i].id, servos[i].offset_angle+servos[i].angle*servos[i].orientation, UNIT_DEGREE);
+    }
+  }
+  
+}
+
+void printServoDebug() {
+  for (uint8_t i=0;i<SERVO_COUNT;i++) {
+    Serial.print("Servo"); Serial.print(i);Serial.print(" : "); Serial.print(dxl.getPresentPosition(servos[i].id, UNIT_DEGREE)); Serial.print(" -> "); Serial.print(servos[i].offset_angle+servos[i].angle);
+    Serial.println();
+  }
+}
+
+
+void scanDynamixel(int32_t baud, uint8_t protocol,uint8_t maxid) { 
+  
+  int8_t found_dynamixel = 0;
+  dxl.setPortProtocolVersion((float)protocol);
+  dxl.begin(baud);
+  for(int id = 0; id < maxid; id++) { //highest ID is DXL_BROADCAST_ID
+    //iterate until all ID in each buadrate is scanned.
+    if(dxl.ping(id)) {
+      DEBUG_SERIAL.print("ID : ");
+      DEBUG_SERIAL.print(id);
+      DEBUG_SERIAL.print(", Model Number: ");
+      DEBUG_SERIAL.println(dxl.getModelNumber(id));
+      found_dynamixel++;
+    }
+  }
+}
+
+void scanDynamixel() 
+{
+  #define MAX_BAUD  5
+  const int32_t buad[MAX_BAUD] = {57600, 115200, 1000000, 2000000, 3000000};
+
+
+  int8_t index = 0;
+  int8_t found_dynamixel = 0;
+
+    
+  for(int8_t protocol = 1; protocol < 3; protocol++) {
+    // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
+    dxl.setPortProtocolVersion((float)protocol);
+    DEBUG_SERIAL.print("SCAN PROTOCOL ");
+    DEBUG_SERIAL.println(protocol);
+    
+    for(index = 0; index < MAX_BAUD; index++) {
+      // Set Port baudrate.
+      DEBUG_SERIAL.print("SCAN BAUDRATE ");
+      DEBUG_SERIAL.println(buad[index]);
+      dxl.begin(buad[index]);
+      for(int id = 0; id < DXL_BROADCAST_ID; id++) {
+        //iterate until all ID in each buadrate is scanned.
+        if(dxl.ping(id)) {
+          DEBUG_SERIAL.print("ID : ");
+          DEBUG_SERIAL.print(id);
+          DEBUG_SERIAL.print(", Model Number: ");
+          DEBUG_SERIAL.println(dxl.getModelNumber(id));
+          found_dynamixel++;
+        }
+      }
+    }
+  }
+  
+  DEBUG_SERIAL.print("Total ");
+  DEBUG_SERIAL.print(found_dynamixel);
+  DEBUG_SERIAL.println(" DYNAMIXEL(s) found!");
+}
+
+
+
+void changeID(uint8_t present_id, uint8_t new_id) {
+  // put your setup code here, to run once:
+  
+  // For Uno, Nano, Mini, and Mega, use UART port of DYNAMIXEL Shield to debug.
+  DEBUG_SERIAL.begin(115200);
+  while(!DEBUG_SERIAL);
+  
+  // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
+  dxl.begin(57600);
+  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
+  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
+
+  DEBUG_SERIAL.print("PROTOCOL ");
+  DEBUG_SERIAL.print(DXL_PROTOCOL_VERSION, 1);
+  DEBUG_SERIAL.print(", ID ");
+  DEBUG_SERIAL.print(present_id);
+  DEBUG_SERIAL.print(": ");
+  if(dxl.ping(present_id) == true) {
+    DEBUG_SERIAL.print("ping succeeded!");
+    DEBUG_SERIAL.print(", Model Number: ");
+    DEBUG_SERIAL.println(dxl.getModelNumber(present_id));
+    
+    // Turn off torque when configuring items in EEPROM area
+    dxl.torqueOff(present_id);
+    
+    // set a new ID for DYNAMIXEL. Do not use ID 200
+    if(dxl.setID(present_id, new_id) == true){
+      present_id = new_id;
+      DEBUG_SERIAL.print("ID has been successfully changed to ");
+      DEBUG_SERIAL.println(new_id);
+    }else{
+      DEBUG_SERIAL.print("Failed to change ID to ");
+      DEBUG_SERIAL.println(new_id);
+    }
+  }
+  else{
+    DEBUG_SERIAL.println("ping failed!");
+  }
+}

servo.h

@@ -2,4 +2,41 @@
 #define SERVO_H_
 
 
+#include <Dynamixel2Arduino.h>
+#include "actuator.h" //needed to be able to access ControlTableItem::
+
+#if defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit.
+  //#define DXL_SERIAL   Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board)
+  #define DXL_SERIAL   Serial1 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board)
+  #define DEBUG_SERIAL Serial
+  //const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board)
+  const int DXL_DIR_PIN = 28; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board)
+#endif
+
+#define DXL_SERIAL   Serial1
+
+#define SERVO_UPDATE_INTERVAL 1000/50
+
+
+struct dxl_servo {
+	int id;
+	//int initial_speed;
+	float offset_angle;
+  float angle;
+	float min_angle;
+	float max_angle;
+	int orientation;
+  uint8_t mode=OP_CURRENT_BASED_POSITION;
+  float current;
+  uint16_t modelnumber;
+};
+
+void scanDynamixel(int32_t baud, uint8_t protocol,uint8_t maxid);
+void scanDynamixel();
+void changeID(uint8_t present_id, uint8_t new_id);
+void initServos();
+void loopServos(unsigned long millis);
+void printServoDebug();
+
+
 #endif 

vacuum.cpp

@@ -45,7 +45,7 @@ void initVacuum() {
 
 void loopVacuum(unsigned long millis) {
   static unsigned long last_pressurereading = 0;
-  if (millis - last_pressurereading > 10)
+  if (millis - last_pressurereading > VACUUM_UPDATE_INTERVAL)
   {
     last_pressurereading=millis;
     vacuum1 = readHX710B(PIN_PRESSURE1_OUT) / 1000000.0;

vacuum.h

@@ -6,7 +6,7 @@
 
 
 
-
+#define VACUUM_UPDATE_INTERVAL 1000/10