set home joint 3+4

Dependencies:   mbed

Committer:
pbdt1997
Date:
Sat Apr 20 17:03:37 2019 +0000
Revision:
2:ee6d40dd8fb6
Parent:
1:5ed21efc1d58
700 baht interrupt driven stepper

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pbdt1997 0:a3b83d874092 1 #include "mbed.h"
pbdt1997 0:a3b83d874092 2
pbdt1997 2:ee6d40dd8fb6 3 InterruptIn prox1(PA_13);
pbdt1997 2:ee6d40dd8fb6 4 InterruptIn prox2(PA_14);
pbdt1997 1:5ed21efc1d58 5 Ticker stepper;
pbdt1997 1:5ed21efc1d58 6 //Ticker stepper2;
pbdt1997 1:5ed21efc1d58 7
pbdt1997 0:a3b83d874092 8 Serial pc(USBTX, USBRX);
pbdt1997 0:a3b83d874092 9
pbdt1997 0:a3b83d874092 10 SPISlave slave(PA_7, PA_6, PA_5, PA_15); //MOSI MISO CLK CS
pbdt1997 0:a3b83d874092 11
pbdt1997 0:a3b83d874092 12 //prox detects = 0
pbdt1997 2:ee6d40dd8fb6 13 //DigitalIn prox2(P/A_14);
pbdt1997 2:ee6d40dd8fb6 14 //DigitalIn prox1(PA_13);
pbdt1997 0:a3b83d874092 15
pbdt1997 0:a3b83d874092 16 //Stepper Motor 1
pbdt1997 0:a3b83d874092 17 DigitalOut PUL_1(D3);
pbdt1997 0:a3b83d874092 18 DigitalOut DR_1(PC_0);
pbdt1997 0:a3b83d874092 19
pbdt1997 0:a3b83d874092 20 //Stepper Motor 2
pbdt1997 0:a3b83d874092 21 DigitalOut PUL_2(D4);
pbdt1997 0:a3b83d874092 22 DigitalOut DR_2(PC_1);
pbdt1997 0:a3b83d874092 23
pbdt1997 1:5ed21efc1d58 24 float q3 = 0, q4 = 0;
pbdt1997 1:5ed21efc1d58 25 float countq3 = 0, countq4 = 0;
pbdt1997 1:5ed21efc1d58 26 bool moveq3 = false, moveq4 = false;
pbdt1997 1:5ed21efc1d58 27 bool stop = false;
pbdt1997 2:ee6d40dd8fb6 28 float stepq3 = 0.0, stepq4 = 0.0;
pbdt1997 2:ee6d40dd8fb6 29 int t = 500;
pbdt1997 1:5ed21efc1d58 30
pbdt1997 0:a3b83d874092 31 //drive stepper motor
pbdt1997 1:5ed21efc1d58 32 void drvStepper1(float angle){
pbdt1997 1:5ed21efc1d58 33 float step = angle - q3;
pbdt1997 1:5ed21efc1d58 34 step = step*44.4444444;
pbdt1997 0:a3b83d874092 35 if(step >= 0){
pbdt1997 1:5ed21efc1d58 36 for(int i=0; i<(int)step; i++){
pbdt1997 0:a3b83d874092 37 DR_1 = 0; //up
pbdt1997 0:a3b83d874092 38 PUL_1 = 1;
pbdt1997 0:a3b83d874092 39 wait_ms(1);
pbdt1997 0:a3b83d874092 40 PUL_1 = 0;
pbdt1997 0:a3b83d874092 41 wait_ms(1);
pbdt1997 1:5ed21efc1d58 42 countq3++;
pbdt1997 0:a3b83d874092 43 }
pbdt1997 0:a3b83d874092 44 }
pbdt1997 0:a3b83d874092 45 else if(step < 0){
pbdt1997 0:a3b83d874092 46 step = -step;
pbdt1997 1:5ed21efc1d58 47 for(int i=0; i<(int)step; i++){
pbdt1997 0:a3b83d874092 48 DR_1 = 1; //down
pbdt1997 0:a3b83d874092 49 PUL_1 = 1;
pbdt1997 0:a3b83d874092 50 wait_ms(1);
pbdt1997 0:a3b83d874092 51 PUL_1 = 0;
pbdt1997 0:a3b83d874092 52 wait_ms(1);
pbdt1997 1:5ed21efc1d58 53 countq3--;
pbdt1997 0:a3b83d874092 54 }
pbdt1997 0:a3b83d874092 55 }
pbdt1997 0:a3b83d874092 56 }
pbdt1997 0:a3b83d874092 57
pbdt1997 1:5ed21efc1d58 58 void drvStepper2(float angle){
pbdt1997 1:5ed21efc1d58 59 // printf("hello");
pbdt1997 1:5ed21efc1d58 60 float step = angle - q4;
pbdt1997 1:5ed21efc1d58 61 step = step*11.1111111;
pbdt1997 0:a3b83d874092 62 if(step >= 0){
pbdt1997 1:5ed21efc1d58 63 for(int i=0; i<(int)step; i++){
pbdt1997 0:a3b83d874092 64 DR_2 = 0; //up
pbdt1997 0:a3b83d874092 65 PUL_2 = 1;
pbdt1997 0:a3b83d874092 66 wait_ms(1);
pbdt1997 0:a3b83d874092 67 PUL_2 = 0;
pbdt1997 0:a3b83d874092 68 wait_ms(1);
pbdt1997 1:5ed21efc1d58 69 countq4++;
pbdt1997 0:a3b83d874092 70 }
pbdt1997 0:a3b83d874092 71 }
pbdt1997 0:a3b83d874092 72 else if(step < 0){
pbdt1997 0:a3b83d874092 73 step = -step;
pbdt1997 1:5ed21efc1d58 74 for(int i=0; i<(int)step; i++){
pbdt1997 0:a3b83d874092 75 DR_2 = 1; //down
pbdt1997 0:a3b83d874092 76 PUL_2 = 1;
pbdt1997 0:a3b83d874092 77 wait_ms(1);
pbdt1997 0:a3b83d874092 78 PUL_2 = 0;
pbdt1997 0:a3b83d874092 79 wait_ms(1);
pbdt1997 1:5ed21efc1d58 80 countq4--;
pbdt1997 0:a3b83d874092 81 }
pbdt1997 0:a3b83d874092 82 }
pbdt1997 0:a3b83d874092 83 }
pbdt1997 0:a3b83d874092 84
pbdt1997 2:ee6d40dd8fb6 85 void convertStep(double angle, char joint){
pbdt1997 2:ee6d40dd8fb6 86 if(joint == 3){
pbdt1997 2:ee6d40dd8fb6 87 stepq3 = angle - q3;
pbdt1997 2:ee6d40dd8fb6 88 stepq3 = stepq3*(44.0 + 4.0/9.0);
pbdt1997 2:ee6d40dd8fb6 89 }
pbdt1997 2:ee6d40dd8fb6 90 else if(joint == 4){
pbdt1997 2:ee6d40dd8fb6 91 stepq4 = angle - q4;
pbdt1997 2:ee6d40dd8fb6 92 stepq4 = stepq4*(1.0 + 1.0/9.0);
pbdt1997 1:5ed21efc1d58 93 }
pbdt1997 2:ee6d40dd8fb6 94 }
pbdt1997 2:ee6d40dd8fb6 95
pbdt1997 2:ee6d40dd8fb6 96 void driveStepper(){
pbdt1997 2:ee6d40dd8fb6 97 //if(moveq3 == true){
pbdt1997 2:ee6d40dd8fb6 98 // stepq3 = stepq3 - q3;
pbdt1997 2:ee6d40dd8fb6 99 // stepq3 = stepq3*(44.0 + 4.0/9.0);
pbdt1997 2:ee6d40dd8fb6 100 // if(countq3 >= stepq3){
pbdt1997 2:ee6d40dd8fb6 101 // moveq3 = false;
pbdt1997 2:ee6d40dd8fb6 102 // }
pbdt1997 2:ee6d40dd8fb6 103 // else{
pbdt1997 2:ee6d40dd8fb6 104 // DR_1 = 1;
pbdt1997 2:ee6d40dd8fb6 105 // PUL_1 = 1;
pbdt1997 2:ee6d40dd8fb6 106 // countq3++;
pbdt1997 2:ee6d40dd8fb6 107 // }
pbdt1997 2:ee6d40dd8fb6 108 // }
pbdt1997 2:ee6d40dd8fb6 109 // else{PUL_1 = 0;}
pbdt1997 1:5ed21efc1d58 110 if(moveq4 == true){
pbdt1997 2:ee6d40dd8fb6 111 if(countq4 >= stepq4){
pbdt1997 2:ee6d40dd8fb6 112 moveq4 = false;
pbdt1997 2:ee6d40dd8fb6 113 }
pbdt1997 2:ee6d40dd8fb6 114 else{
pbdt1997 2:ee6d40dd8fb6 115 DR_2 = 1;
pbdt1997 2:ee6d40dd8fb6 116 PUL_2 = 1;
pbdt1997 2:ee6d40dd8fb6 117 countq4++;
pbdt1997 2:ee6d40dd8fb6 118 }
pbdt1997 1:5ed21efc1d58 119 }
pbdt1997 2:ee6d40dd8fb6 120 else{PUL_2 = 0;}
pbdt1997 1:5ed21efc1d58 121 }
pbdt1997 1:5ed21efc1d58 122
pbdt1997 0:a3b83d874092 123 void setHome(){
pbdt1997 0:a3b83d874092 124 while(prox2 == 1){
pbdt1997 0:a3b83d874092 125 drvStepper2(1);
pbdt1997 0:a3b83d874092 126 }
pbdt1997 0:a3b83d874092 127 wait(0.5);
pbdt1997 1:5ed21efc1d58 128 drvStepper2(-138.51);
pbdt1997 0:a3b83d874092 129 wait(0.5);
pbdt1997 1:5ed21efc1d58 130 countq4 = 0;
pbdt1997 1:5ed21efc1d58 131 q4 = 0;
pbdt1997 0:a3b83d874092 132 while(prox1 == 1){
pbdt1997 0:a3b83d874092 133 drvStepper1(-1);
pbdt1997 0:a3b83d874092 134 }
pbdt1997 0:a3b83d874092 135 wait(0.5);
pbdt1997 1:5ed21efc1d58 136 drvStepper1(18);
pbdt1997 0:a3b83d874092 137 wait(0.5);
pbdt1997 1:5ed21efc1d58 138 countq3 = 0;
pbdt1997 1:5ed21efc1d58 139 q3 = 0;
pbdt1997 0:a3b83d874092 140 }
pbdt1997 0:a3b83d874092 141
pbdt1997 2:ee6d40dd8fb6 142 void stop_q3(){
pbdt1997 2:ee6d40dd8fb6 143 PUL_1 = 0;
pbdt1997 2:ee6d40dd8fb6 144 }
pbdt1997 2:ee6d40dd8fb6 145
pbdt1997 2:ee6d40dd8fb6 146 void stop_q4(){
pbdt1997 2:ee6d40dd8fb6 147 PUL_2 = 0;
pbdt1997 2:ee6d40dd8fb6 148 }
pbdt1997 2:ee6d40dd8fb6 149
pbdt1997 0:a3b83d874092 150 int main() {
pbdt1997 2:ee6d40dd8fb6 151 stepper.attach_us(&driveStepper, t);
pbdt1997 1:5ed21efc1d58 152 // stepper2.attach_us(&drvStepper2, 20);
pbdt1997 2:ee6d40dd8fb6 153 pc.baud(250000);
pbdt1997 2:ee6d40dd8fb6 154 prox1.rise(&stop_q3);
pbdt1997 2:ee6d40dd8fb6 155 prox2.rise(&stop_q4);
pbdt1997 0:a3b83d874092 156 setHome();
pbdt1997 2:ee6d40dd8fb6 157 wait(2);
pbdt1997 2:ee6d40dd8fb6 158 countq3 = 0;
pbdt1997 2:ee6d40dd8fb6 159 countq4 = 0;
pbdt1997 2:ee6d40dd8fb6 160 moveq4 = true;
pbdt1997 2:ee6d40dd8fb6 161 convertStep(5.0, 4);
pbdt1997 2:ee6d40dd8fb6 162 // q3 = 100;
pbdt1997 1:5ed21efc1d58 163 while(1){
pbdt1997 2:ee6d40dd8fb6 164 // printf("%.2f\n",countq3);
pbdt1997 2:ee6d40dd8fb6 165 printf("stepq4 %.2f countq4 %.2f\n",stepq4,countq4);
pbdt1997 2:ee6d40dd8fb6 166 // wait(0.1);
pbdt1997 2:ee6d40dd8fb6 167 // moveq3 = true;
pbdt1997 2:ee6d40dd8fb6 168 // q3 = 10;
pbdt1997 2:ee6d40dd8fb6 169 }
pbdt1997 2:ee6d40dd8fb6 170 // setHome();
pbdt1997 2:ee6d40dd8fb6 171 // printf("q3 = %.2f, q4 = %.2f\n", countq3,countq4);
pbdt1997 2:ee6d40dd8fb6 172 // while(1){
pbdt1997 2:ee6d40dd8fb6 173 // moveq4 = true;
pbdt1997 2:ee6d40dd8fb6 174 // q4 = 10;
pbdt1997 1:5ed21efc1d58 175 // printf("done\n");
pbdt1997 1:5ed21efc1d58 176 // moveq3 = false;
pbdt1997 1:5ed21efc1d58 177 // break;
pbdt1997 2:ee6d40dd8fb6 178 // wait(5);
pbdt1997 2:ee6d40dd8fb6 179 // }
pbdt1997 1:5ed21efc1d58 180 // drvStepper1(2000);
pbdt1997 0:a3b83d874092 181 }