File content as of revision 1:4eeae0f92c4c:

//Module8-9
//Nucleo Board 1 (Master)

#include "mbed.h"
#include <Map.hpp>

Serial pc(USBTX, USBRX);

SPI master(PA_7, PA_6, PA_5);       //SPI1_MOSI, SPI1_MISO, SPI1_SCLK
SPI mstrEnc2(PB_15, PB_14, PB_13);  //SPI2_MOSI, SPI2_MISO, SPI2_SCLK
SPI mstrEnc1(PC_12, PC_11, PC_10);  //SPI3_MOSI, SPI3_MISO, SPI3_SCLK

DigitalOut csMstr(PA_15);           //SPI1_SSEL Master
DigitalOut csEnc2(PB_12);           //SPI2_SSEL Encoder 1
DigitalOut csEnc1(PA_4);            //SPI3_SSEL Encoder 2

DigitalOut dir1(PC_0);
PwmOut pwm1(PC_9);

DigitalOut dir2(PC_1);
PwmOut pwm2(PC_8);

//Parameters

char array[4];
char arraySize = sizeof(array);
char getData = 0;
char checkData = 0;
char encArr[2]= {255,255};
char encLength = 2;
bool start = false;


/////////////////////////////

//Functions

//store data from pc
void RX(int data){
    array[checkData] = data;
    checkData++;
    if(checkData == arraySize){
        getData = 1;
        checkData = 0;
    }
}

//receive data from pc
void RX_INT(){
    int data = pc.getc();
    RX(data);
}

//convert 14bit data to angle
float ConvertAngle(int encData){
    Map map(0, 16383, 0, 359);
    float angle = map.Calculate(encData);
    return angle;
    }

//read encoder of joint 1
float readEnc1(){
    csEnc1 = 0;
    char *arrAdd = &encArr[0];
    int encData = mstrEnc1.write(arrAdd, 2, arrAdd, 2);
    encData = encArr[0] * 256 + encArr[1];
    csEnc1 = 1;
    return ConvertAngle(encData/2);
}

//read encoder of joint 2
float readEnc2(){
    csEnc2 = 0;
    char *arrAdd = &encArr[0];
    int encData = mstrEnc2.write(arrAdd, 2, arrAdd, 2);
    encData = encArr[0] * 256 + encArr[1];
    csEnc2 = 1;
    return ConvertAngle(encData/2);
}

//drive motor joint1
void drvMotor1(float duty){
    if(duty > 0.9){
        duty = 0.9;
    }
    else if(duty < -0.9){
        duty = -0.9;
    }
    if(duty >= 0){
        dir1 = 1; //CW
        
    }
    else if(duty < 0){
        dir1 = 0; //CCW
        duty = -duty;
    }
    pwm1.period_ms(1.0f);
    pwm1.write(duty);
}

//drive motor joint1
void drvMotor2(float duty){
    if(duty > 0.5){
        duty = 0.5;
    }
    else if(duty < -0.5){
        duty = -0.5;
    }
    if(duty >= 0){
        dir2 = 1; //CCW
    }
    else if(duty < 0){
        dir2 = 0; //CW
        duty = -duty;
    }
    pwm2.period_ms(1.0f);
    pwm2.write(duty);
} 

//PID of joint 1
void PID_1(float pos){
    pos = -pos;
    const float Kp=0.5,Ki=0,Kd=0.1;
    float u,e,p,s;
    float offset = 110.66;
    bool stop1 = false;
    
    while(stop1==false){
        float encData1 = readEnc1();
        float encData2 = readEnc2();
        printf("Encoder1: %.2f  ", encData1);
        printf("Encoder2: %.2f\n", encData2);
        if(encData1 < 110){
            encData1 = encData1 + 360;
        }
        e = (pos - encData1) + offset;
        s = s + e;
        if(abs(e)>0.5){
            u = (Kp*e) + (Ki*s) + (Kd*(e-p));
        }
        else{
            u = 0;
            drvMotor1(0);
            stop1 = true;
        }
        drvMotor1(u);
        p = e;
    }
}

//PID of joint 2
void PID_2(float pos){
    const float Kp=1,Ki=0,Kd=0.1;
    float u,e,p,s;
    float offset = 21.52 - 135;
    bool stop = false;
    pos=-pos;
    while(stop==false){
        float encData1 = readEnc1();
        float encData2 = readEnc2();
        printf("Encoder1: %.2f  ", encData1);
        printf("Encoder2: %.2f\n", encData2);
        e = (pos - encData2) + offset;
        s = s + e;
        if(abs(e)>0.5){
            u = (Kp*e) + (Ki*s) + (Kd*(e-p));
        }
        else{
            u = 0;
            drvMotor2(0);
            stop = true;
        }
        drvMotor2(u);
        p = e;
    }
}
//Set Home
void setHome(){
    PID_1(0);
    PID_2(0);
    }

//Main

int main() {
    pc.baud(256000);
    RX_INT();
    pc.attach(&RX_INT, Serial::RxIrq);
    
    csMstr = 1;
    csEnc1 = 1;
    csEnc2 = 1;
    
    master.format(8,3);
    master.frequency(1000000);
    mstrEnc1.format(14, 3);
    mstrEnc1.frequency(500000);
    mstrEnc2.format(14, 3);
    mstrEnc2.frequency(500000);
    
//Program Loop    
    while(true){
        if(getData == 1){
            for(char i = 0; i < arraySize; i++){
                pc.putc(array[i]);
            }
            if(array[2] == 122){ //receive 'z'
                start = true;
                csMstr = 0;
                master.write(array[3]);
                int masterData = master.write(0x00);
                csMstr = 1;
                pc.putc(masterData);
                pc.putc('A');
            }
            if(array[2] == 121){ //'y'
                setHome();
            }
            
            getData = 0;
        }
        //test driving joint 1 and joint 2
              PID_2(45);
        wait(0.5);
        PID_1(90);
        wait(0.1);
        PID_2(30);
        wait(3);
        PID_1(0);
        wait(0.1);
        PID_2(-45);
        wait(100);
        
    }
    
    
    
    
}