File content as of revision 2:ee6d40dd8fb6:

#include "mbed.h"

InterruptIn prox1(PA_13);
InterruptIn prox2(PA_14);
Ticker stepper;
//Ticker stepper2;

Serial pc(USBTX, USBRX);

SPISlave slave(PA_7, PA_6, PA_5, PA_15); //MOSI MISO CLK CS

//prox detects = 0
//DigitalIn prox2(P/A_14);
//DigitalIn prox1(PA_13);

//Stepper Motor 1
DigitalOut PUL_1(D3);
DigitalOut DR_1(PC_0);

//Stepper Motor 2
DigitalOut PUL_2(D4);
DigitalOut DR_2(PC_1);

float q3 = 0, q4 = 0;
float countq3 = 0, countq4 = 0;
bool moveq3 = false, moveq4 = false;
bool stop = false;
float stepq3 = 0.0, stepq4 = 0.0;
int t = 500;

//drive stepper motor
void drvStepper1(float angle){
    float step = angle - q3;
    step = step*44.4444444;
    if(step >= 0){
        for(int i=0; i<(int)step; i++){
            DR_1 = 0; //up
            PUL_1 = 1;
            wait_ms(1);
            PUL_1 = 0;
            wait_ms(1);
            countq3++;
        }
    }
    else if(step < 0){
        step = -step;
        for(int i=0; i<(int)step; i++){
            DR_1 = 1; //down
            PUL_1 = 1;
            wait_ms(1);
            PUL_1 = 0;
            wait_ms(1);
            countq3--;
        }
    }
}

void drvStepper2(float angle){
   // printf("hello");
    float step = angle - q4;
    step = step*11.1111111;
    if(step >= 0){
        for(int i=0; i<(int)step; i++){
            DR_2 = 0; //up
            PUL_2 = 1;
            wait_ms(1);
            PUL_2 = 0;
            wait_ms(1);
            countq4++;
        }
    }
    else if(step < 0){
        step = -step;
        for(int i=0; i<(int)step; i++){
            DR_2 = 1; //down
            PUL_2 = 1;
            wait_ms(1);
            PUL_2 = 0;
            wait_ms(1);
            countq4--;
        }
    }
}

void convertStep(double angle, char joint){
    if(joint == 3){
        stepq3 = angle - q3;
        stepq3 = stepq3*(44.0 + 4.0/9.0);
    }
    else if(joint == 4){
        stepq4 = angle - q4;
        stepq4 = stepq4*(1.0 + 1.0/9.0);
    }
}

void driveStepper(){
    //if(moveq3 == true){
//        stepq3 = stepq3 - q3;
//        stepq3 = stepq3*(44.0 + 4.0/9.0);
//        if(countq3 >= stepq3){
//            moveq3 = false;
//        }
//        else{
//            DR_1 = 1;
//            PUL_1 = 1;
//            countq3++;
//        }
//    }
//    else{PUL_1 = 0;}
    if(moveq4 == true){
        if(countq4 >= stepq4){
            moveq4 = false;
        }
        else{
            DR_2 = 1;
            PUL_2 = 1;
            countq4++;
        }
    }
    else{PUL_2 = 0;}
}

void setHome(){
    while(prox2 == 1){
        drvStepper2(1);
    }
    wait(0.5);
    drvStepper2(-138.51);
    wait(0.5);
    countq4 = 0;
    q4 = 0;
    while(prox1 == 1){
        drvStepper1(-1);
    }
    wait(0.5);
    drvStepper1(18);
    wait(0.5);
    countq3 = 0;
    q3 = 0;
}

void stop_q3(){
    PUL_1 = 0;
}

void stop_q4(){
    PUL_2 = 0;
}

int main() {
    stepper.attach_us(&driveStepper, t);
//    stepper2.attach_us(&drvStepper2, 20);
    pc.baud(250000);
    prox1.rise(&stop_q3);
    prox2.rise(&stop_q4);
    setHome();
    wait(2);
    countq3 = 0;
    countq4 = 0;
    moveq4 = true;
    convertStep(5.0, 4);
//    q3 = 100;
    while(1){
//        printf("%.2f\n",countq3);
        printf("stepq4 %.2f countq4 %.2f\n",stepq4,countq4);
//        wait(0.1);
//        moveq3 = true;
//        q3 = 10;
    }
//    setHome();
//    printf("q3 = %.2f, q4 = %.2f\n", countq3,countq4);
//    while(1){
//    moveq4 = true;
//    q4 = 10;
//    printf("done\n");
//        moveq3 = false;
//        break;
//    wait(5);    
//    }
//    drvStepper1(2000);
}