set home joint 3+4
Dependencies: mbed
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 }
Generated on Fri Jul 29 2022 18:50:52 by
1.7.2