set home joint 3+4

Dependencies:   mbed

Committer:
pbdt1997
Date:
Sun Apr 21 14:06:22 2019 +0000
Revision:
5:d86a0c29dd29
Parent:
4:fb0905390ebc
just pray

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pbdt1997 0:a3b83d874092 1 #include "mbed.h"
pbdt1997 0:a3b83d874092 2
pbdt1997 4:fb0905390ebc 3 InterruptIn prox1(PA_13);
pbdt1997 4:fb0905390ebc 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 //Stepper Motor 1
pbdt1997 0:a3b83d874092 13 DigitalOut PUL_1(D3);
pbdt1997 0:a3b83d874092 14 DigitalOut DR_1(PC_0);
pbdt1997 0:a3b83d874092 15
pbdt1997 0:a3b83d874092 16 //Stepper Motor 2
pbdt1997 0:a3b83d874092 17 DigitalOut PUL_2(D4);
pbdt1997 0:a3b83d874092 18 DigitalOut DR_2(PC_1);
pbdt1997 0:a3b83d874092 19
pbdt1997 5:d86a0c29dd29 20
pbdt1997 5:d86a0c29dd29 21 //globals
pbdt1997 5:d86a0c29dd29 22 double q3 = 0, q4 = 0;
pbdt1997 5:d86a0c29dd29 23 double q3_count = 0, q4_count = 0;
pbdt1997 5:d86a0c29dd29 24 double q3_speed = 1, q4_speed = 1;
pbdt1997 5:d86a0c29dd29 25 double q3_step = 0, q4_step = 0;
pbdt1997 1:5ed21efc1d58 26 bool moveq3 = false, moveq4 = false;
pbdt1997 1:5ed21efc1d58 27 bool stop = false;
pbdt1997 5:d86a0c29dd29 28 double t = 10000.0;
pbdt1997 5:d86a0c29dd29 29
pbdt1997 5:d86a0c29dd29 30 //Functions
pbdt1997 1:5ed21efc1d58 31
pbdt1997 5:d86a0c29dd29 32 //conversion funcs
pbdt1997 5:d86a0c29dd29 33 void convertStep(double angle, char joint){
pbdt1997 5:d86a0c29dd29 34 if(joint == 3){
pbdt1997 5:d86a0c29dd29 35 q3_step = angle - q3;
pbdt1997 5:d86a0c29dd29 36 q3_step = abs(q3_step*(44.444444));
pbdt1997 0:a3b83d874092 37 }
pbdt1997 5:d86a0c29dd29 38 else if(joint == 4){
pbdt1997 5:d86a0c29dd29 39 q4_step = angle - q4;
pbdt1997 5:d86a0c29dd29 40 // q4_step = q4_step*(10.0 + 1.0/9.0);
pbdt1997 5:d86a0c29dd29 41 q4_step = abs(q4_step*(11.1111111));
pbdt1997 0:a3b83d874092 42 }
pbdt1997 0:a3b83d874092 43 }
pbdt1997 0:a3b83d874092 44
pbdt1997 0:a3b83d874092 45
pbdt1997 5:d86a0c29dd29 46 //ISRs//
pbdt1997 4:fb0905390ebc 47
pbdt1997 5:d86a0c29dd29 48 //Proximity sensors
pbdt1997 4:fb0905390ebc 49 void stop_q3(){
pbdt1997 4:fb0905390ebc 50 PUL_1 = 0;
pbdt1997 4:fb0905390ebc 51 }
pbdt1997 4:fb0905390ebc 52
pbdt1997 4:fb0905390ebc 53 void stop_q4(){
pbdt1997 4:fb0905390ebc 54 PUL_2 = 0;
pbdt1997 4:fb0905390ebc 55 }
pbdt1997 4:fb0905390ebc 56
pbdt1997 5:d86a0c29dd29 57 //
pbdt1997 5:d86a0c29dd29 58 void driveStepper(){
pbdt1997 5:d86a0c29dd29 59 if(moveq4 == true){
pbdt1997 5:d86a0c29dd29 60 if(q4_count >= q4_step){
pbdt1997 5:d86a0c29dd29 61 moveq4 = false;
pbdt1997 5:d86a0c29dd29 62 if(DR_2 == 0){
pbdt1997 5:d86a0c29dd29 63 q4 += q4_count/11.1111111;
pbdt1997 5:d86a0c29dd29 64 }else{
pbdt1997 5:d86a0c29dd29 65 q4 -= q4_count/11.1111111;
pbdt1997 5:d86a0c29dd29 66 }
pbdt1997 5:d86a0c29dd29 67 q4_count = 0;
pbdt1997 5:d86a0c29dd29 68 PUL_2 = 0;
pbdt1997 5:d86a0c29dd29 69
pbdt1997 5:d86a0c29dd29 70 }
pbdt1997 5:d86a0c29dd29 71 else{
pbdt1997 5:d86a0c29dd29 72 if((uint8_t)q4_count%(uint8_t)q4_speed==0){
pbdt1997 5:d86a0c29dd29 73 PUL_2 = !PUL_2;
pbdt1997 5:d86a0c29dd29 74 }
pbdt1997 5:d86a0c29dd29 75 q4_count++;
pbdt1997 5:d86a0c29dd29 76
pbdt1997 5:d86a0c29dd29 77 }
pbdt1997 5:d86a0c29dd29 78 }
pbdt1997 5:d86a0c29dd29 79 if(moveq3 == true){
pbdt1997 5:d86a0c29dd29 80 if(q3_count >= q3_step){
pbdt1997 5:d86a0c29dd29 81 moveq3 = false;
pbdt1997 5:d86a0c29dd29 82 if(DR_1 == 0){
pbdt1997 5:d86a0c29dd29 83 q3 += q3_count/44.4444444;
pbdt1997 5:d86a0c29dd29 84 }else{
pbdt1997 5:d86a0c29dd29 85 q3 -= q3_count/44.4444444;
pbdt1997 5:d86a0c29dd29 86 }
pbdt1997 5:d86a0c29dd29 87 q3_count = 0;
pbdt1997 5:d86a0c29dd29 88 PUL_1 = 0;
pbdt1997 5:d86a0c29dd29 89 }
pbdt1997 5:d86a0c29dd29 90 else{
pbdt1997 5:d86a0c29dd29 91 if((uint8_t)q3_count%(uint8_t)q3_speed==0){
pbdt1997 5:d86a0c29dd29 92 PUL_1 = !PUL_1;
pbdt1997 5:d86a0c29dd29 93 }
pbdt1997 5:d86a0c29dd29 94 q3_count++;
pbdt1997 5:d86a0c29dd29 95
pbdt1997 5:d86a0c29dd29 96 }
pbdt1997 5:d86a0c29dd29 97 }
pbdt1997 5:d86a0c29dd29 98 }
pbdt1997 5:d86a0c29dd29 99
pbdt1997 5:d86a0c29dd29 100 void drvStepper1(double angle, double speed){
pbdt1997 5:d86a0c29dd29 101 q3_speed = speed;
pbdt1997 5:d86a0c29dd29 102 if(angle - q3 >= 0){
pbdt1997 5:d86a0c29dd29 103 DR_1 = 0;
pbdt1997 5:d86a0c29dd29 104 }
pbdt1997 5:d86a0c29dd29 105 else{
pbdt1997 5:d86a0c29dd29 106 DR_1 = 1;
pbdt1997 5:d86a0c29dd29 107 }
pbdt1997 5:d86a0c29dd29 108 moveq3 = true;
pbdt1997 5:d86a0c29dd29 109 convertStep(angle, 3);
pbdt1997 5:d86a0c29dd29 110 }
pbdt1997 5:d86a0c29dd29 111
pbdt1997 5:d86a0c29dd29 112 void drvStepper2(double angle, double speed){
pbdt1997 5:d86a0c29dd29 113 // stepper.detach();
pbdt1997 5:d86a0c29dd29 114 // stepper.attach_us(&driveStepper, 1000000/(speed));
pbdt1997 5:d86a0c29dd29 115 q4_speed = speed;
pbdt1997 4:fb0905390ebc 116 if(angle - q4 >= 0){
pbdt1997 4:fb0905390ebc 117 DR_2 = 0;
pbdt1997 5:d86a0c29dd29 118 }
pbdt1997 4:fb0905390ebc 119 else{
pbdt1997 4:fb0905390ebc 120 DR_2 = 1;
pbdt1997 5:d86a0c29dd29 121 }
pbdt1997 1:5ed21efc1d58 122 moveq4 = true;
pbdt1997 4:fb0905390ebc 123 convertStep(angle, 4);
pbdt1997 5:d86a0c29dd29 124 }
pbdt1997 5:d86a0c29dd29 125
pbdt1997 5:d86a0c29dd29 126 void veloControl(double q3_trgt, double q4_trgt, double v3, double v4){
pbdt1997 5:d86a0c29dd29 127 //wait till previous movement stopped
pbdt1997 5:d86a0c29dd29 128 while(moveq4||moveq3){
pbdt1997 5:d86a0c29dd29 129 }
pbdt1997 5:d86a0c29dd29 130 drvStepper1(q3_trgt, v3);
pbdt1997 5:d86a0c29dd29 131 drvStepper2(q4_trgt, v4);
pbdt1997 5:d86a0c29dd29 132 moveq3 = true;
pbdt1997 5:d86a0c29dd29 133 moveq4 = true;
pbdt1997 5:d86a0c29dd29 134 }
pbdt1997 5:d86a0c29dd29 135
pbdt1997 5:d86a0c29dd29 136 void setHome(){
pbdt1997 5:d86a0c29dd29 137 //while(prox2 == 1){
pbdt1997 5:d86a0c29dd29 138 // drvStepper2(1, 1000.0);
pbdt1997 5:d86a0c29dd29 139 // }
pbdt1997 5:d86a0c29dd29 140 // wait(0.5);
pbdt1997 5:d86a0c29dd29 141 // drvStepper2(-138.51);
pbdt1997 5:d86a0c29dd29 142 // wait(0.5);
pbdt1997 5:d86a0c29dd29 143 // q4 = 0;
pbdt1997 5:d86a0c29dd29 144 // q4 = 0;
pbdt1997 5:d86a0c29dd29 145 // while(prox1 == 1){
pbdt1997 5:d86a0c29dd29 146 // drvStepper1(-1);
pbdt1997 5:d86a0c29dd29 147 // }
pbdt1997 5:d86a0c29dd29 148 // wait(0.5);
pbdt1997 5:d86a0c29dd29 149 // drvStepper1(18);
pbdt1997 5:d86a0c29dd29 150 // wait(0.5);
pbdt1997 5:d86a0c29dd29 151 // q3 = 0;
pbdt1997 5:d86a0c29dd29 152 // q3 = 0;
pbdt1997 5:d86a0c29dd29 153 }
pbdt1997 5:d86a0c29dd29 154
pbdt1997 5:d86a0c29dd29 155
pbdt1997 5:d86a0c29dd29 156
pbdt1997 4:fb0905390ebc 157
pbdt1997 4:fb0905390ebc 158
pbdt1997 4:fb0905390ebc 159 int main() {
pbdt1997 5:d86a0c29dd29 160 stepper.attach_us(&driveStepper, 100.0);
pbdt1997 4:fb0905390ebc 161 pc.baud(250000);
pbdt1997 4:fb0905390ebc 162 //prox1.rise(&stop_q3);
pbdt1997 4:fb0905390ebc 163 // prox2.rise(&stop_q4);
pbdt1997 4:fb0905390ebc 164 moveq4 = false;
pbdt1997 5:d86a0c29dd29 165 moveq3 = false;
pbdt1997 5:d86a0c29dd29 166 // setHome();
pbdt1997 5:d86a0c29dd29 167 // wait(2);
pbdt1997 5:d86a0c29dd29 168 q3 = 0;
pbdt1997 5:d86a0c29dd29 169 q4 = 0;
pbdt1997 4:fb0905390ebc 170
pbdt1997 5:d86a0c29dd29 171 veloControl(10,10,10,10);
pbdt1997 5:d86a0c29dd29 172 printf("q3 = %.2f q4 = %.2f\n", q3, q4);
pbdt1997 5:d86a0c29dd29 173 while(moveq4||moveq3){
pbdt1997 5:d86a0c29dd29 174 printf("numba 1 q3 %.2f q4 %.2f\n",q3_count,q4_count);
pbdt1997 5:d86a0c29dd29 175 }
pbdt1997 5:d86a0c29dd29 176 printf("q3 = %.2f q4 = %.2f\n", q3, q4);
pbdt1997 5:d86a0c29dd29 177 veloControl(-10,-15,10,10);
pbdt1997 4:fb0905390ebc 178
pbdt1997 5:d86a0c29dd29 179 while(moveq4||moveq3){
pbdt1997 5:d86a0c29dd29 180 printf("numba 2 q3 %.2f q4 %.2f\n",q3_count,q4_count);
pbdt1997 5:d86a0c29dd29 181 }
pbdt1997 5:d86a0c29dd29 182 printf("q3 = %.2f q4 = %.2f\n", q3, q4);
pbdt1997 5:d86a0c29dd29 183 // while(1){
pbdt1997 5:d86a0c29dd29 184 //// drvStepper2(60.0, 1000);
pbdt1997 5:d86a0c29dd29 185 //// wait(5);
pbdt1997 5:d86a0c29dd29 186 //// drvStepper2(30.0, 1000);
pbdt1997 5:d86a0c29dd29 187 // printf("q4_count %.2f q4 %.2f\n",q4_count,q4);
pbdt1997 5:d86a0c29dd29 188 // wait_ms(10);
pbdt1997 5:d86a0c29dd29 189 //
pbdt1997 5:d86a0c29dd29 190 // }
pbdt1997 4:fb0905390ebc 191
pbdt1997 0:a3b83d874092 192 }