Revision 11:19e8022f60ea, committed 2016-07-13

Comitter:

clynamen

Date:

Wed Jul 13 15:53:34 2016 +0000

Parent:

10:2acfa1a84c96

Commit message:

do fixes;

Changed in this revision

--- a/AX12.lib	Sat Jul 02 18:06:59 2016 +0000
+++ b/AX12.lib	Wed Jul 13 15:53:34 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/Team-DIANA/code/AX12_final/#9532fc2ca8b1
+https://developer.mbed.org/teams/Team-DIANA/code/AX12_final/#4487db847117

--- a/Joint.h	Sat Jul 02 18:06:59 2016 +0000
+++ b/Joint.h	Wed Jul 13 15:53:34 2016 +0000
@@ -8,19 +8,18 @@
     public:
     virtual ~Joint() {};
     virtual void setID(int NewID) = 0;
+    virtual int getID() = 0;
     // TODO: use enum instead of int
     virtual void setMode(int mode) = 0; 
     virtual void setCWLimit(float degrees) = 0;
     virtual void setCCWLimit(float degrees) = 0;
     virtual void setGoalPosition(float degrees) = 0;
     virtual float getPosition() = 0;
-    virtual void setSpeed(float goal_speed) = 0;
+    virtual void setSpeed(float degreesS) = 0;
+    virtual void setMaxSpeed(float degreeS) = 0;
     virtual float getTemp() = 0;
     virtual float getVolts() = 0;
     virtual float getCurrent () = 0;
-    virtual float getPGain () = 0;
-    virtual float getIGain () = 0;
-    virtual float getDGain() = 0;
     virtual void setMaxTorque(float torque) = 0 ;
     virtual void setMotorEnabled(bool enabled) = 0;
     virtual bool isMoving() = 0;

--- a/MX.cpp	Sat Jul 02 18:06:59 2016 +0000
+++ b/MX.cpp	Wed Jul 13 15:53:34 2016 +0000
@@ -2,11 +2,15 @@
 
 #include <cmath>
 
-MX::MX(communication_1& line, int ID, float gear_train) : 
-  _line(line), 
-  _ID(ID),
-  _gear_train(gear_train) {
-  
+#define MOVING_SPEED_UNIT_RPM 0.114
+
+MX::MX(communication_1& line, int ID, float gear_train)
+    : _line(line)
+    , _ID(ID)
+    , _gear_train(gear_train) {
+      maxSpeedDegreeS = 360;
+      limitCWDegrees = 0;
+      limitCCWDegrees = 0;
 }
 
 MX::~MX() {
@@ -14,50 +18,49 @@
 
 void MX::setID(int newID) {
 
-  char data[1];
-  data[0] = newID;
+    char data[1];
+    data[0] = newID;
+
+    if (MX_DEBUG) {
+        printf("Setting ID from 0x%x to 0x%x\n", _ID, newID);
+    }
 
-  if (MX_DEBUG) {
-    printf("Setting ID from 0x%x to 0x%x\n", _ID, newID);
-  }
+    _line.write(_ID, MX_REG_ID, 1, data);
+    _ID = newID;
+}
 
-  _line.write(_ID, MX_REG_ID, 1, data);
-  _ID = newID;
+int MX::getID() {
+  return _ID;
 }
 
 void MX::setGoalPosition(float degrees) {
+    setMode(2);
+    setCRSpeed(maxSpeedDegreeS);
     setGoal(degrees);
 }
 
 int MX::setGoal(float degrees, int flags) {
-if((_mode==0)&&((degrees<0)||(degrees>360))) 
-    {
-    if(MX_DEBUG==1) 
-    {  
-    printf("Error, wheel mode ");
-    }
-    return(1);
-        
+    if ((_mode == 0) && ((degrees < 0) || (degrees > 360))) {
+        if (MX_DEBUG == 1) {
+            printf("Error, wheel mode ");
+        }
+        return (1);
     }
-if (_mode == 1) 
-    { 
-    if(MX_DEBUG==1) 
-    {  
-    printf("Error, wheel mode ");
-    }
-    return(1);
-        
+
+    if (_mode == 1) {
+        if (MX_DEBUG == 1) {
+            printf("Error, wheel mode ");
+        }
+        return (1);
     }
-    if((_mode==2)&&((degrees<0)||(degrees>360)))
-    {
-       if(MX_DEBUG==1) 
-    {  
-    printf("Error, wheel mode ");
-    }
-    return(1);
-       
-    }  
-    
+
+    //if ((_mode == 2) && ((degrees < 0) || (degrees > 360))) {
+        //if (MX_DEBUG == 1) {
+            //printf("Error, wheel mode ");
+        //}
+        //return (1);
+    //}
+
     char reg_flag = 0;
     char data[2];
 
@@ -66,10 +69,15 @@
         reg_flag = 1;
     }
 
-    
-    short  goal = (int)(degrees/goal_resolution );     /// chiedere se può andar bene short int****
+    if (degrees > limitCWDegrees && limitCWDegrees != 0) {
+      degrees = limitCWDegrees;
+    } else if (degrees < limitCCWDegrees && limitCCWDegrees != 0){
+      degrees = limitCCWDegrees;
+    }
+
+    uint16_t goal = (int)(degrees * _gear_train / goal_resolution); 
     if (MX_DEBUG) {
-        printf("SetGoal to 0x%x\n",goal);
+        printf("SetGoal to 0x%x\n", goal);
     }
 
     data[0] = goal & 0xff; // bottom 8 bits
@@ -77,42 +85,83 @@
 
     // write the packet, return the error code
     int rVal = _line.write(_ID, MX_REG_GOAL_POSITION, 2, data, reg_flag);
-    
+
     if (flags == 1) {
         // block until it comes to a halt
-        while (isMoving()) {}
+        while (isMoving()) {
+        }
     }
 }
 
-float MX::getVolts (void) {
+float MX::getVolts(void) {
     if (MX_DEBUG) {
-        printf("\nGetVolts(%d)",_ID);
+        printf("\nGetVolts(%d)", _ID);
     }
     char data[1];
     int ErrorCode = _line.read(_ID, MX_REG_VOLTS, 1, data);
-    float volts = data[0]/10.0;
-    return(volts);
+    float volts = data[0] / 10.0;
+    return (volts);
 }
 
-float MX::getCurrent(void)
-{
+float MX::getCurrent(void) {
     if (MX_DEBUG) {
         printf("\nGetCurrent(%d)", _ID);
     }
     char data[1];
     int ErrorCode = _line.read(_ID, MX_REG_CURRENT, 1, data);
-    float ampere  = 4.5 * (data[0] - 2048 )  ;
-    return(ampere);
-    
+    float ampere = 4.5 * (data[0] - 2048);
+    return (ampere);
+}
+
+float MX::getPGain() {
+    char data[1];
+    _line.read(_ID, MX_REG_PGAIN, 1, data);
+    return (float) *data / 8;
+}
+
+float MX::getIGain() {
+    char data[1];
+    _line.read(_ID, MX_REG_IGAIN, 1, data);
+    return (float) *data * 1000 / 2048;
+}
+
+float MX::getDGain() {
+    char data[1];
+    _line.read(_ID, MX_REG_DGAIN, 1, data);
+    return (float) *data * 4 / 1000;
 }
 
-
+void MX::setPGain(float gain) {
+    char data[1];
+    *data = gain * 8;
+    _line.write(_ID, MX_REG_PGAIN, 1, data);
+}
+void MX::setIGain(float gain) {
+    char data[1];
+    *data = gain / 1000 * 2048;
+    _line.write(_ID, MX_REG_IGAIN, 1, data);
+}
+void MX::setDGain(float gain) {
+    char data[1];
+    *data = gain / 4 * 1000;
+    _line.write(_ID, MX_REG_DGAIN, 1, data);
+}
 
-float MX::getPGain() { return 0; }
-float MX::getIGain() { return 0; }
-float MX::getDGain() { return 0; }
-void MX::setMaxTorque(float torque) { } 
-void MX::setMotorEnabled(bool enabled) { } 
+void MX::setMaxTorque(float maxTorque) {
+    char data[2];
+    short torqueValue = maxTorque * 0x3ff; 
+    data[0] = torqueValue & 0xff; // bottom 8 bits
+    data[1] = torqueValue >> 8;   // top 8 bits
+    // write the packet, return the error code
+    _line.write(_ID, MX_REG_MAXTORQUE, 2, data);
+}
+
+void MX::setMotorEnabled(bool enabled) {
+    char data[1];
+    data[0] = enabled; // bottom 8 bits
+    // write the packet, return the error code
+    _line.write(_ID, MX_REG_MOTORONOFF, 1, data);
+}
 
 float MX::getPosition(void) {
     if (MX_DEBUG) {
@@ -123,7 +172,7 @@
 
     int ErrorCode = _line.read(_ID, MX_REG_POSITION, 2, data);
     short position = data[0] + (data[1] << 8);
-    float angle = (position * 300)/4093;
+    float angle = (float)position * 0.088;
 
     return (angle);
 }
@@ -131,112 +180,120 @@
 
 bool MX::isMoving() {
     char data[1];
-    _line.read(_ID,MX_REG_MOVING,1,data);
-    return(data[0]);
+    _line.read(_ID, MX_REG_MOVING, 1, data);
+    return (data[0]);
 }
 
-void MX::setMode(int mode){
-  switch (mode){
-    //Wheel Mode
-    case (0): 
-      setCWLimit(0);
-      setCCWLimit(0);
-      setCRSpeed(0.0);
-      break;
-      //Joint Mode
-    case (1): 
-      setCWLimit(MX_RESOLUTION);
-      setCCWLimit(MX_RESOLUTION);
-      setCRSpeed(0.0);
-      break;
-      //Multi-turn Mode 
-    case (2): 
-      setCWLimit(360);
-      setCCWLimit(360);
-      setCRSpeed(0.0);
-      break;
-      //other cases
-    default: 
-      if(READ_DEBUG){
-        printf("Not valid mode"); 
-        return;
-      }   
-  }
-  _mode = mode;
+void MX::setMode(int mode) {
+    switch (mode) {
+        //Wheel Mode
+        case (0):
+            setCWLimitUnits(0);
+            setCCWLimitUnits(0);
+            //setCRSpeed(0.0);
+            break;
+        //Joint Mode
+        case (1):
+            setCWLimitUnits(4000);
+            setCCWLimitUnits(4000);
+            //setCRSpeed(0.0);
+            break;
+        //Multi-turn Mode
+        case (2):
+            setCWLimitUnits(4095);
+            setCCWLimitUnits(4095);
+            //setCRSpeed(0.0);
+            break;
+        //other cases
+        default:
+            if (READ_DEBUG) {
+                printf("Not valid mode");
+                return;
+            }
+    }
+    _mode = mode;
 }
 
-void MX::setCWLimit(float degrees){
-  char data[2];
-
-  short limit = (short)(MX_BIT_DEG * degrees * _gear_train);
-
-  data[0] = limit & 0xff; // bottom 8 bits
-  data[1] = limit >> 8;   // top 8 bits
-
-  // write the packet, return the error code
-  _line.write(_ID, MX_REG_CW_LIMIT, 2, data);
+void MX::setCWLimitUnits(short limit) {
+    char data[2];
+    data[0] = limit & 0xff; // bottom 8 bits
+    data[1] = limit >> 8;   // top 8 bits
+    // write the packet, return the error code
+    _line.write(_ID, MX_REG_CW_LIMIT, 2, data);
+}
+void MX::setCCWLimitUnits(short limit) {
+    char data[2];
+    data[0] = limit & 0xff; // bottom 8 bits
+    data[1] = limit >> 8;   // top 8 bits
+    // write the packet, return the error code
+    _line.write(_ID, MX_REG_CCW_LIMIT, 2, data);
 }
 
-void MX::setCCWLimit(float degrees){
-  char data[2];
-
-  // 1023 / 300 * degrees
-  short limit = (4093 * degrees) / 300;
+void MX::setCWLimit(float degrees) {
+    limitCWDegrees = degrees;
+    //short limit = (short)(MX_BIT_DEG * degrees * _gear_train);
+    //setCWLimit(limit);
+}
 
-  if (MX_DEBUG) {
-    printf("SetCCWLimit to 0x%x\n",limit);
-  }
-
-  data[0] = limit & 0xff; // bottom 8 bits
-  data[1] = limit >> 8;   // top 8 bits
-
-  // write the packet, return the error code
-  _line.write(_ID, MX_REG_CCW_LIMIT, 2, data);
+void MX::setCCWLimit(float degrees) {
+    limitCCWDegrees = degrees;
+    //short limit = (short)(MX_BIT_DEG * degrees * _gear_train);
+    //setCCWLimit(limit);
 }
 
 
-void MX::setSpeed(float goal_speed) {
+void MX::setSpeed(float goalSpeedDegreeS) {
+  setMode(0);
+  setCRSpeed(goalSpeedDegreeS);
+}
 
+void MX::setMaxSpeed(float maxSpeedDegreeS) {
+  this->maxSpeedDegreeS = maxSpeedDegreeS;
+}
+
+void MX::setCRSpeed(float goalSpeedDegreeS) {
     // bit 10     = direction, 0 = CCW, 1=CW
     // bits 9-0   = Speed
     char data[2];
 
-    int goal = (0x3ff * std::abs(goal_speed * _gear_train));
+    float rpm = std::abs(goalSpeedDegreeS * _gear_train) / 360 * 60;
+    int goal = rpm / MOVING_SPEED_UNIT_RPM;
+    goal = fmin(goal, 0x3ff);
 
     // Set direction CW if we have a negative speed
-    if (goal_speed < 0) {
+    if (goalSpeedDegreeS < 0) {
         goal |= (0x1 << 10);
     }
 
     data[0] = goal & 0xff; // bottom 8 bits
     data[1] = goal >> 8;   // top 8 bits
-    
+
     // write the packet, return the error code
     _line.write(_ID, MX_REG_MOVING_SPEED, 2, data);
 }
 
-void MX::setCRSpeed(float speed) {
-    // bit 10     = direction, 0 = CCW, 1=CW
-    // bits 9-0   = Speed
-    char data[2];
+//void MX::setCRSpeed(float speed) {
+    //// bit 10     = direction, 0 = CCW, 1=CW
+    //// bits 9-0   = Speed
+    //char data[2];
 
-    int goal = (0x3ff * std::abs(speed));
+    //int goal = (0x3ff * std::abs(speed));
 
-    // Set direction CW if we have a negative speed
-    if (speed < 0) {
-        goal |= (0x1 << 10);
-    }
+    //// Set direction CW if we have a negative speed
+    //if (speed < 0) {
+        //goal |= (0x1 << 10);
+    //}
 
-    data[0] = goal & 0xff; // bottom 8 bits
-    data[1] = goal >> 8;   // top 8 bits
-    
-    // write the packet, return the error code
-    int rVal = _line.write( _ID, 0x20, 2, data);
-}
+    //data[0] = goal & 0xff; // bottom 8 bits
+    //data[1] = goal >> 8;   // top 8 bits
+
+    //// write the packet, return the error code
+    //int rVal = _line.write(_ID, 0x20, 2, data);
+//}
 
 float MX::getTemp(void) {
     char data[1];
-    int ErrorCode = _line.read( _ID, MX_REG_TEMP, 1, data);
+    int ErrorCode = _line.read(_ID, MX_REG_TEMP, 1, data);
     float temp = data[0];
-    return(temp);
+    return (temp);
 }

--- a/MX.h	Sat Jul 02 18:06:59 2016 +0000
+++ b/MX.h	Wed Jul 13 15:53:34 2016 +0000
@@ -61,13 +61,18 @@
     MX(communication_1& line, int ID, float gear_train);
     virtual ~MX();
     void setID(int newID);
+    int getID();
 
     /** Set the mode of the servo
      * @param mode
      *    0 = Positional, default
      *    1 = Continuous rotation
      */
-    void setMode(int mode);
+    virtual void setMode(int mode);
+
+    void setCWLimitUnits(short limit);
+    void setCCWLimitUnits(short limit);
+
     /** Set the clockwise limit of the servo
      *
      * @param degrees, 0-300
@@ -78,13 +83,15 @@
      * @param degrees, 0-300
      */
     void setCCWLimit(float degrees);
+
     /** Set the speed of the servo in continuous rotation mode
      *
      * @param speed, -1.0 to 1.0
      *   -1.0 = full speed counter clock wise
      *    1.0 = full speed clock wise
      */
-    void setCRSpeed(float speed);
+    //void setCRSpeed(float speed);
+    //
     void setGoalPosition(float degrees);
     float getPosition();
 
@@ -97,13 +104,18 @@
      *
      */
     int setGoal(float degrees, int flags = 0);
-    void setSpeed(float goal_speed);
+    void setMaxSpeed(float degreeS);
+    void setCRSpeed(float degreeS);
+    void setSpeed(float degreeS);
     float getTemp();
     float getCurrent();
     float getVolts();
-    float getPGain();
-    float getIGain();
-    float getDGain();
+    virtual float getPGain();
+    virtual float getIGain();
+    virtual float getDGain();
+    virtual void setPGain(float gain);
+    virtual void setIGain(float gain);
+    virtual void setDGain(float gain);
     void setMaxTorque(float torque);
     void setMotorEnabled(bool enabled);
     bool isMoving();
@@ -113,6 +125,9 @@
     float _gear_train;
     int _ID;
     int _mode;
+    float maxSpeedDegreeS;
+    float limitCWDegrees;
+    float limitCCWDegrees;;
 };
 
 #endif 

--- a/MX106.lib	Sat Jul 02 18:06:59 2016 +0000
+++ b/MX106.lib	Wed Jul 13 15:53:34 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/Team-DIANA/code/MX106_not_working/#adeaefc32a41
+https://developer.mbed.org/teams/Team-DIANA/code/MX106_not_working/#5cf492c37a0e

--- a/communication_1.lib	Sat Jul 02 18:06:59 2016 +0000
+++ b/communication_1.lib	Wed Jul 13 15:53:34 2016 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/Team-DIANA/code/comunication_1/#33d025a403d5
+http://developer.mbed.org/teams/Team-DIANA/code/comunication_1/#8933ad3f9bc2

--- a/main.cpp	Sat Jul 02 18:06:59 2016 +0000
+++ b/main.cpp	Wed Jul 13 15:53:34 2016 +0000
@@ -2,6 +2,10 @@
 #include "communication_1.h"
 #include "MX106.h"
 #include "MX64.h"
+#include <stdio.h>
+#include <string.h>
+
+#define MAX_JOINT 3 
 
 /*Connessioni jupers millefori/nucleo: 
     - rosso 5V
@@ -13,8 +17,138 @@
 Serial pc(USBTX, USBRX);
 communication_1 wire(PA_9, PA_10, 9600);
 MX106 motor_21(wire, 21, 1);
-MX64 motor_22(wire, 22, 1);
+MX106 motor_22(wire, 22, 1);
+MX106 motor_14(wire, 14, 1);
    
 int main() {  
-   pc.printf("Start run! \n");
+   //pc.printf("Start run! \n");
+   wire.trigger();
+   wire.trigger();
+   wire.trigger();
+   wire.trigger();
+   
+   //printf("asking volts\n");
+   //float volts = motor_21.getVolts();
+   //printf("volts: %f\n", volts);
+   
+   motor_21.setMotorEnabled(1);
+
+   //printf("set continuous rotation mode\n");
+   //motor_21.setMode(0);
+   //printf("set speed\n");
+   //motor_21.setSpeed(180);
+   //printf("speed set\n");
+
+
+   //printf("set multiturn mode\n");
+   //motor_21.setMode(2);
+   //printf("asking pos\n");
+   //float pos = motor_21.getPosition();
+   //printf(" pos : %f \n", pos);
+   //motor_21.setSpeed(90);
+   //float wait_time = 5;
+   //motor_21.setGoalPosition(0);
+   //wait(10);
+   //float targets[7] = {0, 90, 180, 270, 180, 360, 720};
+   //for (int i = 0; i < 7; i++) {
+     //motor_21.setGoalPosition(targets[i]);
+     //wait(wait_time);
+   //}
+   //printf("asking pos after move\n");
+   //pos = motor_21.getPosition();
+   //printf(" pos : %f \n", pos);
+
+   Joint* joint_21 = &motor_21;
+
+   float pos;
+   //joint_21->setMaxSpeed(360*3/2);
+   //joint_21->setSpeed(90);
+   //wait(3);
+   //joint_21->setSpeed(0);
+   //wait(1);
+   //pos = motor_21.getPosition();
+   //printf(" pos : %f \n", pos);
+   
+
+   //joint_21->setCWLimit(90);
+   //joint_21->setCCWLimit(-90);
+   //joint_21->setGoalPosition(0); 
+   //wait(5);
+   //pos = motor_21.getPosition();
+   //printf(" pos : %f \n", pos);
+
+   //joint_21->setGoalPosition(80); 
+   //wait(5);
+   //pos = motor_21.getPosition();
+   //printf(" pos : %f \n", pos);
+
+   //joint_21->setGoalPosition(130); 
+   //wait(5);
+   //pos = motor_21.getPosition();
+   //printf(" pos : %f \n", pos);
+
+   //joint_21->setGoalPosition(-130); 
+   //wait(5);
+   //pos = motor_21.getPosition();
+   //printf(" pos : %f \n", pos);
+   //
+   //
+   
+   Joint* joints[MAX_JOINT];
+   joints[0] = &motor_21;
+   joints[1] = &motor_22;
+   joints[2] = &motor_14;
+
+   // comandi
+   char buf[200];
+   uint8_t current_motor = 0; 
+
+   while(1) {
+     printf("\n insert command: \n");
+     if(pc.gets(buf, 200)) {
+       char command[3];
+       strncpy(command, buf, 2);
+       command[2]='\0';
+
+       if(strcmp(command, "ID")  == 0) {
+         int id;
+         if(sscanf(buf, "ID %d", &id) != 1) {
+           printf("\n bad ID command \n");
+         };
+         bool found = false;
+         for(int i =0; i< MAX_JOINT && !found; i++) {
+           int nid = joints[i]->getID();
+           printf("found motor with id %d\n", nid);
+           if(id == nid) {
+             printf("\nusing motor %d - ID: %d", i, id);
+             current_motor = i;
+             found = true;
+             break;
+           } 
+         }
+         if(!found) {
+           printf("\nmotor with ID: %d not found", id);
+         }
+       } else if(strcmp(command, "GO") == 0) {
+         float posDegrees;
+         if(sscanf(buf, "GO %f", &posDegrees) != 1) {
+           printf("\n bad GO command \n");
+           break;
+         }
+         joints[current_motor]->setGoalPosition(posDegrees);
+       } else if(strcmp(command, "SP") == 0) {
+         float speed;
+         if(sscanf(buf, "SP %f", &speed) != 1) {
+           printf("\n bad SP command \n");
+           break;
+         }
+         joints[current_motor]->setSpeed(speed);
+       } else {
+         printf("\n unknown command: \"\s\" \n", buf);
+       }
+
+
+     };
+   }
+
 }