set home joint 3+4
Dependencies: mbed
main.cpp
- Committer:
- pbdt1997
- Date:
- 2019-04-20
- Revision:
- 2:ee6d40dd8fb6
- Parent:
- 1:5ed21efc1d58
File content as of revision 2:ee6d40dd8fb6:
#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 //prox detects = 0 //DigitalIn prox2(P/A_14); //DigitalIn prox1(PA_13); //Stepper Motor 1 DigitalOut PUL_1(D3); DigitalOut DR_1(PC_0); //Stepper Motor 2 DigitalOut PUL_2(D4); DigitalOut DR_2(PC_1); float q3 = 0, q4 = 0; float countq3 = 0, countq4 = 0; bool moveq3 = false, moveq4 = false; bool stop = false; float stepq3 = 0.0, stepq4 = 0.0; int t = 500; //drive stepper motor void drvStepper1(float angle){ float step = angle - q3; step = step*44.4444444; if(step >= 0){ for(int i=0; i<(int)step; i++){ DR_1 = 0; //up PUL_1 = 1; wait_ms(1); PUL_1 = 0; wait_ms(1); countq3++; } } else if(step < 0){ step = -step; for(int i=0; i<(int)step; i++){ DR_1 = 1; //down PUL_1 = 1; wait_ms(1); PUL_1 = 0; wait_ms(1); countq3--; } } } void drvStepper2(float angle){ // printf("hello"); float step = angle - q4; step = step*11.1111111; if(step >= 0){ for(int i=0; i<(int)step; i++){ DR_2 = 0; //up PUL_2 = 1; wait_ms(1); PUL_2 = 0; wait_ms(1); countq4++; } } else if(step < 0){ step = -step; for(int i=0; i<(int)step; i++){ DR_2 = 1; //down PUL_2 = 1; wait_ms(1); PUL_2 = 0; wait_ms(1); countq4--; } } } void convertStep(double angle, char joint){ if(joint == 3){ stepq3 = angle - q3; stepq3 = stepq3*(44.0 + 4.0/9.0); } else if(joint == 4){ stepq4 = angle - q4; stepq4 = stepq4*(1.0 + 1.0/9.0); } } void driveStepper(){ //if(moveq3 == true){ // stepq3 = stepq3 - q3; // stepq3 = stepq3*(44.0 + 4.0/9.0); // if(countq3 >= stepq3){ // moveq3 = false; // } // else{ // DR_1 = 1; // PUL_1 = 1; // countq3++; // } // } // else{PUL_1 = 0;} if(moveq4 == true){ if(countq4 >= stepq4){ moveq4 = false; } else{ DR_2 = 1; PUL_2 = 1; countq4++; } } else{PUL_2 = 0;} } void setHome(){ while(prox2 == 1){ drvStepper2(1); } wait(0.5); drvStepper2(-138.51); wait(0.5); countq4 = 0; q4 = 0; while(prox1 == 1){ drvStepper1(-1); } wait(0.5); drvStepper1(18); wait(0.5); countq3 = 0; q3 = 0; } void stop_q3(){ PUL_1 = 0; } void stop_q4(){ PUL_2 = 0; } int main() { stepper.attach_us(&driveStepper, t); // stepper2.attach_us(&drvStepper2, 20); pc.baud(250000); prox1.rise(&stop_q3); prox2.rise(&stop_q4); setHome(); wait(2); countq3 = 0; countq4 = 0; moveq4 = true; convertStep(5.0, 4); // q3 = 100; while(1){ // printf("%.2f\n",countq3); printf("stepq4 %.2f countq4 %.2f\n",stepq4,countq4); // wait(0.1); // moveq3 = true; // q3 = 10; } // setHome(); // printf("q3 = %.2f, q4 = %.2f\n", countq3,countq4); // while(1){ // moveq4 = true; // q4 = 10; // printf("done\n"); // moveq3 = false; // break; // wait(5); // } // drvStepper1(2000); }