File content as of revision 5:d86a0c29dd29:

#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

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

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


//globals
double q3 = 0, q4 = 0;
double q3_count = 0, q4_count = 0;
double q3_speed = 1, q4_speed = 1;
double q3_step = 0, q4_step = 0;
bool moveq3 = false, moveq4 = false;
bool stop = false;
double t = 10000.0;

//Functions

//conversion funcs
void convertStep(double angle, char joint){
    if(joint == 3){
        q3_step = angle - q3;
        q3_step = abs(q3_step*(44.444444));
    }
    else if(joint == 4){
        q4_step = angle - q4;
//        q4_step = q4_step*(10.0 + 1.0/9.0);
        q4_step = abs(q4_step*(11.1111111));
    }
}


//ISRs//

//Proximity sensors
void stop_q3(){
    PUL_1 = 0;
}

void stop_q4(){
    PUL_2 = 0;
}

//
void driveStepper(){
    if(moveq4 == true){
        if(q4_count >= q4_step){
            moveq4 = false;
            if(DR_2 == 0){
                q4 += q4_count/11.1111111;
                }else{
                q4 -= q4_count/11.1111111;
                }
            q4_count = 0;
            PUL_2 = 0;

        }
        else{
            if((uint8_t)q4_count%(uint8_t)q4_speed==0){
            PUL_2 = !PUL_2;
            }
            q4_count++;

        }
    }
    if(moveq3 == true){
        if(q3_count >= q3_step){
            moveq3 = false;
            if(DR_1 == 0){
                q3 += q3_count/44.4444444;
            }else{
                q3 -= q3_count/44.4444444;
            }
            q3_count = 0;
            PUL_1 = 0;
        }
        else{
            if((uint8_t)q3_count%(uint8_t)q3_speed==0){
            PUL_1 = !PUL_1;
            }
            q3_count++;

        }
    }
}

void drvStepper1(double angle, double speed){
    q3_speed = speed;
    if(angle - q3 >= 0){
        DR_1 = 0;
    }
    else{
        DR_1 = 1;
    }
        moveq3 = true;
    convertStep(angle, 3);
}

void drvStepper2(double angle, double speed){
//    stepper.detach();
//    stepper.attach_us(&driveStepper, 1000000/(speed));
    q4_speed = speed;
    if(angle - q4 >= 0){
        DR_2 = 0;
    }
    else{
        DR_2 = 1;
    }
    moveq4 = true;
    convertStep(angle, 4);
}

void veloControl(double q3_trgt, double q4_trgt, double v3, double v4){
    //wait till previous movement stopped
    while(moveq4||moveq3){
        }
    drvStepper1(q3_trgt, v3);
    drvStepper2(q4_trgt, v4);
    moveq3 = true;
    moveq4 = true;
}

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



    

int main() {
    stepper.attach_us(&driveStepper, 100.0);
    pc.baud(250000);
    //prox1.rise(&stop_q3);
//    prox2.rise(&stop_q4);
    moveq4 = false;
    moveq3 = false;
//    setHome();
//    wait(2);
    q3 = 0;
    q4 = 0;
    
    veloControl(10,10,10,10);
    printf("q3 = %.2f q4 = %.2f\n", q3, q4);
    while(moveq4||moveq3){
        printf("numba 1 q3 %.2f q4 %.2f\n",q3_count,q4_count);
        }
    printf("q3 = %.2f q4 = %.2f\n", q3, q4);
    veloControl(-10,-15,10,10);

       while(moveq4||moveq3){
        printf("numba 2 q3 %.2f q4 %.2f\n",q3_count,q4_count);
        }
    printf("q3 = %.2f q4 = %.2f\n", q3, q4);
 //   while(1){
////        drvStepper2(60.0, 1000);
////        wait(5);
////        drvStepper2(30.0, 1000);
//        printf("q4_count %.2f q4 %.2f\n",q4_count,q4);
//        wait_ms(10);
//
//    }

}