set home joint 3+4

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 
00003 InterruptIn prox1(PA_13);
00004 InterruptIn prox2(PA_14);
00005 Ticker stepper;
00006 //Ticker stepper2;
00007 
00008 Serial pc(USBTX, USBRX);
00009 
00010 SPISlave slave(PA_7, PA_6, PA_5, PA_15); //MOSI MISO CLK CS
00011 
00012 //prox detects = 0
00013 //DigitalIn prox2(P/A_14);
00014 //DigitalIn prox1(PA_13);
00015 
00016 //Stepper Motor 1
00017 DigitalOut PUL_1(D3);
00018 DigitalOut DR_1(PC_0);
00019 
00020 //Stepper Motor 2
00021 DigitalOut PUL_2(D4);
00022 DigitalOut DR_2(PC_1);
00023 
00024 float q3 = 0, q4 = 0;
00025 float countq3 = 0, countq4 = 0;
00026 bool moveq3 = false, moveq4 = false;
00027 bool stop = false;
00028 float stepq3 = 0.0, stepq4 = 0.0;
00029 int t = 500;
00030 
00031 //drive stepper motor
00032 void drvStepper1(float angle){
00033     float step = angle - q3;
00034     step = step*44.4444444;
00035     if(step >= 0){
00036         for(int i=0; i<(int)step; i++){
00037             DR_1 = 0; //up
00038             PUL_1 = 1;
00039             wait_ms(1);
00040             PUL_1 = 0;
00041             wait_ms(1);
00042             countq3++;
00043         }
00044     }
00045     else if(step < 0){
00046         step = -step;
00047         for(int i=0; i<(int)step; i++){
00048             DR_1 = 1; //down
00049             PUL_1 = 1;
00050             wait_ms(1);
00051             PUL_1 = 0;
00052             wait_ms(1);
00053             countq3--;
00054         }
00055     }
00056 }
00057 
00058 void drvStepper2(float angle){
00059    // printf("hello");
00060     float step = angle - q4;
00061     step = step*11.1111111;
00062     if(step >= 0){
00063         for(int i=0; i<(int)step; i++){
00064             DR_2 = 0; //up
00065             PUL_2 = 1;
00066             wait_ms(1);
00067             PUL_2 = 0;
00068             wait_ms(1);
00069             countq4++;
00070         }
00071     }
00072     else if(step < 0){
00073         step = -step;
00074         for(int i=0; i<(int)step; i++){
00075             DR_2 = 1; //down
00076             PUL_2 = 1;
00077             wait_ms(1);
00078             PUL_2 = 0;
00079             wait_ms(1);
00080             countq4--;
00081         }
00082     }
00083 }
00084 
00085 void convertStep(double angle, char joint){
00086     if(joint == 3){
00087         stepq3 = angle - q3;
00088         stepq3 = stepq3*(44.0 + 4.0/9.0);
00089     }
00090     else if(joint == 4){
00091         stepq4 = angle - q4;
00092         stepq4 = stepq4*(1.0 + 1.0/9.0);
00093     }
00094 }
00095 
00096 void driveStepper(){
00097     //if(moveq3 == true){
00098 //        stepq3 = stepq3 - q3;
00099 //        stepq3 = stepq3*(44.0 + 4.0/9.0);
00100 //        if(countq3 >= stepq3){
00101 //            moveq3 = false;
00102 //        }
00103 //        else{
00104 //            DR_1 = 1;
00105 //            PUL_1 = 1;
00106 //            countq3++;
00107 //        }
00108 //    }
00109 //    else{PUL_1 = 0;}
00110     if(moveq4 == true){
00111         if(countq4 >= stepq4){
00112             moveq4 = false;
00113         }
00114         else{
00115             DR_2 = 1;
00116             PUL_2 = 1;
00117             countq4++;
00118         }
00119     }
00120     else{PUL_2 = 0;}
00121 }
00122 
00123 void setHome(){
00124     while(prox2 == 1){
00125         drvStepper2(1);
00126     }
00127     wait(0.5);
00128     drvStepper2(-138.51);
00129     wait(0.5);
00130     countq4 = 0;
00131     q4 = 0;
00132     while(prox1 == 1){
00133         drvStepper1(-1);
00134     }
00135     wait(0.5);
00136     drvStepper1(18);
00137     wait(0.5);
00138     countq3 = 0;
00139     q3 = 0;
00140 }
00141 
00142 void stop_q3(){
00143     PUL_1 = 0;
00144 }
00145 
00146 void stop_q4(){
00147     PUL_2 = 0;
00148 }
00149 
00150 int main() {
00151     stepper.attach_us(&driveStepper, t);
00152 //    stepper2.attach_us(&drvStepper2, 20);
00153     pc.baud(250000);
00154     prox1.rise(&stop_q3);
00155     prox2.rise(&stop_q4);
00156     setHome();
00157     wait(2);
00158     countq3 = 0;
00159     countq4 = 0;
00160     moveq4 = true;
00161     convertStep(5.0, 4);
00162 //    q3 = 100;
00163     while(1){
00164 //        printf("%.2f\n",countq3);
00165         printf("stepq4 %.2f countq4 %.2f\n",stepq4,countq4);
00166 //        wait(0.1);
00167 //        moveq3 = true;
00168 //        q3 = 10;
00169     }
00170 //    setHome();
00171 //    printf("q3 = %.2f, q4 = %.2f\n", countq3,countq4);
00172 //    while(1){
00173 //    moveq4 = true;
00174 //    q4 = 10;
00175 //    printf("done\n");
00176 //        moveq3 = false;
00177 //        break;
00178 //    wait(5);    
00179 //    }
00180 //    drvStepper1(2000);
00181 }