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 //Stepper Motor 1
00013 DigitalOut PUL_1(D3);
00014 DigitalOut DR_1(PC_0);
00015 
00016 //Stepper Motor 2
00017 DigitalOut PUL_2(D4);
00018 DigitalOut DR_2(PC_1);
00019 
00020 
00021 //globals
00022 double q3 = 0, q4 = 0;
00023 double q3_count = 0, q4_count = 0;
00024 double q3_speed = 1, q4_speed = 1;
00025 double q3_step = 0, q4_step = 0;
00026 bool moveq3 = false, moveq4 = false;
00027 bool stop = false;
00028 double t = 10000.0;
00029 
00030 //Functions
00031 
00032 //conversion funcs
00033 void convertStep(double angle, char joint){
00034     if(joint == 3){
00035         q3_step = angle - q3;
00036         q3_step = abs(q3_step*(44.444444));
00037     }
00038     else if(joint == 4){
00039         q4_step = angle - q4;
00040 //        q4_step = q4_step*(10.0 + 1.0/9.0);
00041         q4_step = abs(q4_step*(11.1111111));
00042     }
00043 }
00044 
00045 
00046 //ISRs//
00047 
00048 //Proximity sensors
00049 void stop_q3(){
00050     PUL_1 = 0;
00051 }
00052 
00053 void stop_q4(){
00054     PUL_2 = 0;
00055 }
00056 
00057 //
00058 void driveStepper(){
00059     if(moveq4 == true){
00060         if(q4_count >= q4_step){
00061             moveq4 = false;
00062             if(DR_2 == 0){
00063                 q4 += q4_count/11.1111111;
00064                 }else{
00065                 q4 -= q4_count/11.1111111;
00066                 }
00067             q4_count = 0;
00068             PUL_2 = 0;
00069 
00070         }
00071         else{
00072             if((uint8_t)q4_count%(uint8_t)q4_speed==0){
00073             PUL_2 = !PUL_2;
00074             }
00075             q4_count++;
00076 
00077         }
00078     }
00079     if(moveq3 == true){
00080         if(q3_count >= q3_step){
00081             moveq3 = false;
00082             if(DR_1 == 0){
00083                 q3 += q3_count/44.4444444;
00084             }else{
00085                 q3 -= q3_count/44.4444444;
00086             }
00087             q3_count = 0;
00088             PUL_1 = 0;
00089         }
00090         else{
00091             if((uint8_t)q3_count%(uint8_t)q3_speed==0){
00092             PUL_1 = !PUL_1;
00093             }
00094             q3_count++;
00095 
00096         }
00097     }
00098 }
00099 
00100 void drvStepper1(double angle, double speed){
00101     q3_speed = speed;
00102     if(angle - q3 >= 0){
00103         DR_1 = 0;
00104     }
00105     else{
00106         DR_1 = 1;
00107     }
00108         moveq3 = true;
00109     convertStep(angle, 3);
00110 }
00111 
00112 void drvStepper2(double angle, double speed){
00113 //    stepper.detach();
00114 //    stepper.attach_us(&driveStepper, 1000000/(speed));
00115     q4_speed = speed;
00116     if(angle - q4 >= 0){
00117         DR_2 = 0;
00118     }
00119     else{
00120         DR_2 = 1;
00121     }
00122     moveq4 = true;
00123     convertStep(angle, 4);
00124 }
00125 
00126 void veloControl(double q3_trgt, double q4_trgt, double v3, double v4){
00127     //wait till previous movement stopped
00128     while(moveq4||moveq3){
00129         }
00130     drvStepper1(q3_trgt, v3);
00131     drvStepper2(q4_trgt, v4);
00132     moveq3 = true;
00133     moveq4 = true;
00134 }
00135 
00136 void setHome(){
00137     //while(prox2 == 1){
00138 //        drvStepper2(1, 1000.0);
00139 //    }
00140 //    wait(0.5);
00141 //    drvStepper2(-138.51);
00142 //    wait(0.5);
00143 //    q4 = 0;
00144 //    q4 = 0;
00145 //    while(prox1 == 1){
00146 //        drvStepper1(-1);
00147 //    }
00148 //    wait(0.5);
00149 //    drvStepper1(18);
00150 //    wait(0.5);
00151 //    q3 = 0;
00152 //    q3 = 0;
00153 }
00154 
00155 
00156 
00157     
00158 
00159 int main() {
00160     stepper.attach_us(&driveStepper, 100.0);
00161     pc.baud(250000);
00162     //prox1.rise(&stop_q3);
00163 //    prox2.rise(&stop_q4);
00164     moveq4 = false;
00165     moveq3 = false;
00166 //    setHome();
00167 //    wait(2);
00168     q3 = 0;
00169     q4 = 0;
00170     
00171     veloControl(10,10,10,10);
00172     printf("q3 = %.2f q4 = %.2f\n", q3, q4);
00173     while(moveq4||moveq3){
00174         printf("numba 1 q3 %.2f q4 %.2f\n",q3_count,q4_count);
00175         }
00176     printf("q3 = %.2f q4 = %.2f\n", q3, q4);
00177     veloControl(-10,-15,10,10);
00178 
00179        while(moveq4||moveq3){
00180         printf("numba 2 q3 %.2f q4 %.2f\n",q3_count,q4_count);
00181         }
00182     printf("q3 = %.2f q4 = %.2f\n", q3, q4);
00183  //   while(1){
00184 ////        drvStepper2(60.0, 1000);
00185 ////        wait(5);
00186 ////        drvStepper2(30.0, 1000);
00187 //        printf("q4_count %.2f q4 %.2f\n",q4_count,q4);
00188 //        wait_ms(10);
00189 //
00190 //    }
00191 
00192 }