File content as of revision 9:34f4eb572c47:

#include <iostream>
#include "SerialHalfDuplex.h"
#include <AX12.h>
#include <communication_1.h>
#include <MX64.h>
#include "mbed.h"
#define MX_REG_ID 0x3
#define MX_REG_CW_LIMIT 0x06
#define MX_REG_CCW_LIMIT 0x08
#define MX_REG_GOAL_POSITION 0x1E
#define MX_REG_MOVING_SPEED 0x20
#define REG_VOLTS 0x2A
#define MX_REG_AMPERE 0x44
#define MX_REG_TEMP 0x2B
#define MX_REG_PGAIN 0x1C
#define MX_REG_IGAIN 0x1B
#define MX_REG_DGAIN 0X1A
#define MX_REG_MOVING 0x2E
#define MX_REG_MAXTORQUE 0x22
#define MX_REG_POSITION 0x24
#define MX_RESOLUTION 0,088
//bit per degrees 4095/360°
#define MX_REG_MOTORONOFF 0x18
#define MX_BIT_DEG 11,375
#define MX_DEBUG 1
#define MX_TRIGGER_DEBUG 1

#define MX_READ_DEBUG 1
#define AX12_WRITE_DEBUG 1
#define AX12_READ_DEBUG 1
#define AX12_TRIGGER_DEBUG 1
#define AX12_DEBUG 1

#define AX12_REG_ID 0x3
#define AX12_REG_CW_LIMIT 0x06
#define AX12_REG_CCW_LIMIT 0x08
#define AX12_REG_GOAL_POSITION 0x1E
#define AX12_REG_MOVING_SPEED 0x20
#define AX12_REG_VOLTS 0x2A
#define AX12_REG_TEMP 0x2B
#define AX12_REG_MOVING 0x2E
#define AX12_REG_POSITION 0x24
#define AX12_REG_MAXTORQUE 0x22
#define AX12_REG_MOTORONOFF 0x18
#define AX12_MODE_POSITION  0
#define AX12_MODE_ROTATION  1

#define AX12_CW 1
#define AX12_CCW 1
using namespace std;

// La prima lettera di una classe di solito ha la lettera Maiuscola -> Joint
class joint
{
    public:
    // general non serve, le classi astratte pure non dovrebbero avere un costruttore (al costruttore ci pensa la classe child)
    virtual void general (PinName tx, PinName rx, int ID){};
    
    virtual void setID(int CurrentID, int NewID){};
    virtual void setMode(int mode) {};
    virtual void setCWLimit(float degrees) {};
    virtual void setCCWLimit(float degrees){};
    // utilizziamo sempre lo stesso formato in una classe per i nomi dei metodi: lettera minuscola all'inizio
    // goalPosition
    virtual void GoalPosition(float degrees){};
    virtual void SetSpeed(float goal_speed){};
    // GetTemp serve a ottenere la temperature del motore... ma il metodo restituisce void
    // deve restituire un float
    virtual void GetTemp(){};
    // lo stesso per gli altri metodi get
    virtual void GetVolts(){};
    virtual void GetCurrent (){};
    virtual void GetPGain (){};
    virtual void GetIGain (){};
    virtual void GetDGain(){};
    virtual void SetMaxTorque (float torque){};
    virtual void MotorOnOff (){};
    virtual void trigger(){};
    virtual void ismoving(){};
}

// Il codice seguente mischia dichiarazione e definizione. La dichiarazione (nomi, parametri) dovrebbe stare in un file .h
// l'implementazione in un file cpp


class MX : public joint
{
    public:
        // Qui il costruttore e` giusto invece: MX non e` piu` una classe astratta, ma una normale classe parent 
        // Baudrate dovrebbe essere un parametro del costruttore
        // La classe non deve prendere piu tx e rx, ma soltanto un communication
        void general(PinName tx, PinName rx, int ID)
       : _mx(tx,rx) {

    _mx.baud(9600);
    _ID = ID;
    // Questi parametri devono essere letti da un file  (preferibile) o inseriti nel codice.
    // Digitarli dal terminale non e` pratico :)
    cout<<inserisci gear train: ;
    cin>>_gear_train;
}

void setID(int ID){
    
    char data[1];
    data[0] = NewID;
    if (MX_DEBUG) {
        printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
    }
    return (_line.write(CurrentID, MX_REG_ID, 1, data));
}
    
void setMode(int mode){
    switch (mode){
        //Wheel Mode
        case (0): 
            SetCWLimit(0);
            SetCCWLimit(0);
            SetCRSpeed(0.0);
            _mode = mode; 
            break;
        //Joint Mode
        case (1): 
            SetCWLimit(MX_RESOLUTION);
            SetCCWLimit(MX_RESOLUTION);
            SetCRSpeed(0.0);
            _mode = mode;
            break;
        //Multi-turn Mode 
        case (2): 
            SetCWLimit(360);
            SetCCWLimit(360);
            SetCRSpeed(0.0);
            _mode = mode;
            break;
        //other cases
        default: 
            if(READ_DEBUG){
            printf("Not valid mode"); 
            }   
    }
    return(0);
}
void 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
    return(_line.write(_ID, MX_REG_CW_LIMIT, 2, data));
}
void setCCWLimit(float degrees){
    char data[2];

    // 1023 / 300 * degrees
    short limit = (4093 * degrees) / 300;

    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
    return (_line.write(_ID, MX_REG_CCW_LIMIT, 2, data));
}


    }
void GoalPosition(float degrees){
     char data[2];
    
    short gaol_position = (short)(MX_BIT_DEG * degrees * _gear_train);

    data[0] = gaol_position & 0xff; // bottom 8 bits
    data[1] = gaol_position >> 8;   // top 8 bits

    // write the packet, return the error code
    return(_line.write(_ID, MX_REG_GOAL_POSITION, 2, data));
    }
void SetSpeed(float goal_speed){
      // bit 10     = direction, 0 = CCW, 1=CW
    // bits 9-0   = Speed
    char data[2];

    int goal = (0x3ff * abs(speed * _gear_train));

    // 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
    return(_line.write(_ID, MX_REG_MOVING_SPEED, 2, data));
}
void GetTemp(){
    char data[1];
    int ErrorCode = _line.read(_ID, MX_REG_TEMP, 1, data);
    float temp = data[0];
    return(temp);
}
void GetVolts(){
    char data[1];
    int ErrorCode = _line.read(_ID, MX_REG_VOLTS, 1, data);
    float volts= data[1]/10.0;
    return volts;
    
    }
void GetCurrent(){
    char data[1];
    int ErrorCode = _line.read(_ID, MX_REG_AMPERE, 1, data);
    float ampere=(4.5)*(data[1]–2048);
    return ampere;//the result is expressed in mA
    }
void GetPGain(){

     char data[1];
    int ErrorCode = _line.read(_ID, MX_REG_PGAIN,1,data);
    float Kp= data[1]/8;
    return Kp;
    }
void GetIGain(){
     char data[1];
    int ErrorCode = _line.read(_ID,MX_REG_IGAIN,1,data);
    float Ki=(data[1]*1000)/2048;
    return Ki;
    }
void GetDGain(){
    char data[1];
    int ErrorCode = _line.read(_ID, MX_REG_DGAIN,1,data);
    float Kd=(data[1]*4)/1000;
    return Kd;
    }
void SetMaxTorque (float torque){
    char data[2];
    float read_value=(torque * 1023); //what I have to read in register if I want a torque of #torque Nm
    data[0] = read_value & 0xff; // bottom 8 bits
    data[1] = read_value >> 8;   // top 8 bits
    int wrinting= _line.write(_ID, MX_REG_MAXTORQUE, 2, data);
    //BE CAREFULLY!!! if the power is turned on this torque is used as initial value
    //BE CAREFULLY N.2!! If the function of Alarm Shutdown is triggered, the motor loses its torque because the value becomes 0.
    //At this moment, if the value is changed to the value other than 0, the motor can be used again.
    }
void MotorOnOff(){
char data[1];
    int read_value=_line.read(_ID, MX_REG_MOTORONOFF,1,data);
    float result=data[1];
    return(result);
}
void trigger(){
    char TxBuf[16];
    char sum = 0;

    if (MX_TRIGGER_DEBUG) {
        printf("\nTriggered\n");
    }

    // Build the TxPacket first in RAM, then we'll send in one go
    if (MX_TRIGGER_DEBUG) {
        printf("\nTrigger Packet\n  Header : 0xFF, 0xFF\n");
    }

    TxBuf[0] = 0xFF;
    TxBuf[1] = 0xFF;

    // ID - Broadcast
    TxBuf[2] = 0xFE;
    sum += TxBuf[2];

    if (MX_TRIGGER_DEBUG) {
        printf("  ID : %d\n",TxBuf[2]);
    }

    // Length
    TxBuf[3] = 0x02;
    sum += TxBuf[3];
    if (MX_TRIGGER_DEBUG) {
        printf("  Length %d\n",TxBuf[3]);
    }

    // Instruction - ACTION
    TxBuf[4] = 0x04;
    sum += TxBuf[4];
    if (MX_TRIGGER_DEBUG) {
        printf("  Instruction 0x%X\n",TxBuf[5]);
    }

    // Checksum
    TxBuf[5] = 0xFF - sum;
    if (MX_TRIGGER_DEBUG) {
        printf("  Checksum 0x%X\n",TxBuf[5]);
    }

    // Transmit the packet in one burst with no pausing
    for (int i = 0; i < 6 ; i++) {
        _mx.putc(TxBuf[i]);
    }

    // This is a broadcast packet, so there will be no reply

    return(0);
}
void ismoving(){
    
    char data[1];
    _line.read(_ID,MX_REG_MOVING,1,data);
    return(data[0]);
}
    private:
    comunication_1 _line;
    int _ID;
    float _gear_train;
    SerialHalfDuplex _mx;
    }
    }
    
class AX:public joint{
    public:
void general(PinName tx, PinName rx, int ID)
        : _ax12(tx,rx) {
            _ax12.baud(9600);
    _ID = ID;
}
void setID(int CurrentID, int NewID){
    
     char data[1];
    data[0] = NewID;
    if (AX12_DEBUG) {
        printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
    }
    return (_line.write(CurrentID, AX12_REG_ID, 1, data));

}
void setMode(int mode) {
    if (mode == 1) { // set CR
        SetCWLimit(0);
        SetCCWLimit(0);
        SetCRSpeed(0.0);
    } else {
        SetCWLimit(0);
        SetCCWLimit(360);
        SetCRSpeed(0.0);
    }
    return(0);
}
void setCWLimit (int degrees) {

    char data[2];
    
    // 1023 / 300 * degrees
    short limit = (4093 * degrees) / 300;

    if (AX12_DEBUG) {
        printf("SetCWLimit 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
    return (_line.write(_ID, AX12_REG_CW_LIMIT, 2, data));

}
void setCCWLimit(float degrees){
    char data[2];

    // 1023 / 300 * degrees
    short limit = (4093 * degrees) / 300;

    if (AX12_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
    return (_line.write(_ID, AX12_REG_CCW_LIMIT, 2, data));
}
void GoalPosition(float degrees){
    // if flag[0] is set, were blocking
// if flag[1] is set, we're registering
// they are mutually exclusive operations
int flag;
cout<<Inserisci flag: ;
cin<<flag;
int reg_flag=0;
if (flags == 0x2) {
        reg_flag = 1;
    }

    // 1023 / 300 * degrees
    short goal = (4093 * degrees) / 300;
    if (AX12_DEBUG) {
        printf("SetGoal to 0x%x\n",goal);
    }

    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, AX12_REG_GOAL_POSITION, 2, data, reg_flag);

    if (flags == 1) {
        // block until it comes to a halt
        while (isMoving()) {}
    }
    return(rVal);
}
void SetSpeed(float goal_speed){
    // bit 10     = direction, 0 = CCW, 1=CW
    // bits 9-0   = Speed
    char data[2];

    int goal = (0x3ff * abs(goal_speed));

    // Set direction CW if we have a negative speed
    if (goal_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, AX12_REG_MOVING_SPEED, 2, data);

    return(rVal);
}
void GetTemp(){
    if (AX12_DEBUG) {
        printf("\nGetTemp(%d)",_ID);
    }
    char data[1];
    int ErrorCode = _line.read(_ID, AX12_REG_TEMP, 1, data);
    float temp = data[0];
    return(temp);
}
void GetVolts(){
     if (AX12_DEBUG) {
        printf("\nGetVolts(%d)",_ID);
    }
    char data[1];
    int ErrorCode = _line.read(_ID, AX12_REG_VOLTS, 1, data);
    float volts = data[0]/10.0;
    return(volts);
}
void GetCurrent(){
    cout<<Errore!In questo tipo di motore non e possibile rilevare la corrente;
    return(1);
    }
void GetPGain(){
    cout<<Errore!Questo tipo di motore non può avere un guadagno di questo tipo;
    return(1);
    }
void GetIGain(){
    cout<<Errore!Questo tipo di motore non può avere un guadagno di questo tipo;
    return(1);
    }
void GetDGain(){
    cout<<Errore!Questo tipo di motore non può avere un guadagno di questo tipo;
    return(1);
    }
void SetMaxTorque (float torque){
    char data[2];
    float read_value=(torque * 1023); //what I have to read in register if I want a torque of #torque Nm
    data[0] = read_value & 0xff; // bottom 8 bits
    data[1] = read_value >> 8;   // top 8 bits
    int wrinting= _line.write(_ID, AX12_REG_MAXTORQUE, 2, data);
    //BE CAREFULLY!!! if the power is turned on this torque is used as initial value
    //BE CAREFULLY N.2!! If the function of Alarm Shutdown is triggered, the motor loses its torque because the value becomes 0.
    //At this moment, if the value is changed to the value other than 0, the motor can be used again.
    }
void MotorOnOff (){
     char data[1];
    int read_value=_line.read(_ID, AX12_REG_MOTORONOFF,1,data);
    float result=data[1];
    return(result);
}
void trigger(){
     char TxBuf[16];
    char sum = 0;

    if (AX12_TRIGGER_DEBUG) {
        printf("\nTriggered\n");
    }

    // Build the TxPacket first in RAM, then we'll send in one go
    if (AX12_TRIGGER_DEBUG) {
        printf("\nTrigger Packet\n  Header : 0xFF, 0xFF\n");
    }

    TxBuf[0] = 0xFF;
    TxBuf[1] = 0xFF;

    // ID - Broadcast
    TxBuf[2] = 0xFE;
    sum += TxBuf[2];

    if (AX12_TRIGGER_DEBUG) {
        printf("  ID : %d\n",TxBuf[2]);
    }

    // Length
    TxBuf[3] = 0x02;
    sum += TxBuf[3];
    if (AX12_TRIGGER_DEBUG) {
        printf("  Length %d\n",TxBuf[3]);
    }

    // Instruction - ACTION
    TxBuf[4] = 0x04;
    sum += TxBuf[4];
    if (AX12_TRIGGER_DEBUG) {
        printf("  Instruction 0x%X\n",TxBuf[5]);
    }

    // Checksum
    TxBuf[5] = 0xFF - sum;
    if (AX12_TRIGGER_DEBUG) {
        printf("  Checksum 0x%X\n",TxBuf[5]);
    }

    // Transmit the packet in one burst with no pausing
    for (int i = 0; i < 6 ; i++) {
        _ax12.putc(TxBuf[i]);
    }

    // This is a broadcast packet, so there will be no reply

    return;
}
void ismoving(){
     char data[1];
    read(_ID,AX12_REG_MOVING,1,data);
    return(data[0]);
}


    private:
    communication_1 _line;
    int _ID;
    SerialHalfDuplex _ax12;
}


// Stessa cosa: la classe stepper deve avere due files: .h e .cpp
class stepper:public joint{
    public:
void GetPGain(){
    cout<<Errore!Il motore stepper non può avere un guadagno di questo tipo;
    return(1);
    }
void GetIGain(){
    cout<<Errore!Il motore stepper non può avere un guadagno di questo tipo;
    return(1);
    }
void GetDGain(){
    cout<<Errore!Il motore stepper non può avere un guadagno di questo tipo;
    return(1);
    }