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 //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 }
Generated on Thu Jul 28 2022 19:13:22 by
1.7.2