Robotics Studio / Mbed 2 deprecated ghost

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@4:fb0905390ebc, 2019-04-20 (annotated)

Committer:
pbdt1997
Date:
Sat Apr 20 19:32:22 2019 +0000
Revision:
4:fb0905390ebc
Parent:
1:5ed21efc1d58
Child:
5:d86a0c29dd29
oh my goodness, we could do it. yahhhhhh!!!

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 1:5ed21efc1d58 20 float q3 = 0, q4 = 0;
pbdt1997 1:5ed21efc1d58 21 float countq3 = 0, countq4 = 0;
pbdt1997 1:5ed21efc1d58 22 bool moveq3 = false, moveq4 = false;
pbdt1997 1:5ed21efc1d58 23 bool stop = false;
pbdt1997 4:fb0905390ebc 24 float stepq3 = 0.0, stepq4 = 0.0;
pbdt1997 4:fb0905390ebc 25 float t = 10000.0;
pbdt1997 1:5ed21efc1d58 26
pbdt1997 0:a3b83d874092 27 //drive stepper motor
pbdt1997 1:5ed21efc1d58 28 void drvStepper1(float angle){
pbdt1997 1:5ed21efc1d58 29 float step = angle - q3;
pbdt1997 1:5ed21efc1d58 30 step = step*44.4444444;
pbdt1997 0:a3b83d874092 31 if(step >= 0){
pbdt1997 1:5ed21efc1d58 32 for(int i=0; i<(int)step; i++){
pbdt1997 0:a3b83d874092 33 DR_1 = 0; //up
pbdt1997 0:a3b83d874092 34 PUL_1 = 1;
pbdt1997 0:a3b83d874092 35 wait_ms(1);
pbdt1997 0:a3b83d874092 36 PUL_1 = 0;
pbdt1997 0:a3b83d874092 37 wait_ms(1);
pbdt1997 4:fb0905390ebc 38 // countq3++;
pbdt1997 0:a3b83d874092 39 }
pbdt1997 0:a3b83d874092 40 }
pbdt1997 0:a3b83d874092 41 else if(step < 0){
pbdt1997 0:a3b83d874092 42 step = -step;
pbdt1997 1:5ed21efc1d58 43 for(int i=0; i<(int)step; i++){
pbdt1997 0:a3b83d874092 44 DR_1 = 1; //down
pbdt1997 0:a3b83d874092 45 PUL_1 = 1;
pbdt1997 0:a3b83d874092 46 wait_ms(1);
pbdt1997 0:a3b83d874092 47 PUL_1 = 0;
pbdt1997 0:a3b83d874092 48 wait_ms(1);
pbdt1997 4:fb0905390ebc 49 // countq3--;
pbdt1997 0:a3b83d874092 50 }
pbdt1997 0:a3b83d874092 51 }
pbdt1997 0:a3b83d874092 52 }
pbdt1997 0:a3b83d874092 53
pbdt1997 1:5ed21efc1d58 54 void drvStepper2(float angle){
pbdt1997 1:5ed21efc1d58 55 // printf("hello");
pbdt1997 1:5ed21efc1d58 56 float step = angle - q4;
pbdt1997 1:5ed21efc1d58 57 step = step*11.1111111;
pbdt1997 0:a3b83d874092 58 if(step >= 0){
pbdt1997 1:5ed21efc1d58 59 for(int i=0; i<(int)step; i++){
pbdt1997 0:a3b83d874092 60 DR_2 = 0; //up
pbdt1997 0:a3b83d874092 61 PUL_2 = 1;
pbdt1997 0:a3b83d874092 62 wait_ms(1);
pbdt1997 0:a3b83d874092 63 PUL_2 = 0;
pbdt1997 0:a3b83d874092 64 wait_ms(1);
pbdt1997 4:fb0905390ebc 65 // countq4++;
pbdt1997 0:a3b83d874092 66 }
pbdt1997 0:a3b83d874092 67 }
pbdt1997 0:a3b83d874092 68 else if(step < 0){
pbdt1997 0:a3b83d874092 69 step = -step;
pbdt1997 1:5ed21efc1d58 70 for(int i=0; i<(int)step; i++){
pbdt1997 0:a3b83d874092 71 DR_2 = 1; //down
pbdt1997 0:a3b83d874092 72 PUL_2 = 1;
pbdt1997 0:a3b83d874092 73 wait_ms(1);
pbdt1997 0:a3b83d874092 74 PUL_2 = 0;
pbdt1997 0:a3b83d874092 75 wait_ms(1);
pbdt1997 4:fb0905390ebc 76 // countq4--;
pbdt1997 0:a3b83d874092 77 }
pbdt1997 0:a3b83d874092 78 }
pbdt1997 0:a3b83d874092 79 }
pbdt1997 0:a3b83d874092 80
pbdt1997 4:fb0905390ebc 81 void convertStep(double angle, char joint){
pbdt1997 4:fb0905390ebc 82 if(joint == 3){
pbdt1997 4:fb0905390ebc 83 stepq3 = angle - q3;
pbdt1997 4:fb0905390ebc 84 stepq3 = stepq3*(44.0 + 4.0/9.0);
pbdt1997 4:fb0905390ebc 85 }
pbdt1997 4:fb0905390ebc 86 else if(joint == 4){
pbdt1997 4:fb0905390ebc 87 stepq4 = angle - q4;
pbdt1997 4:fb0905390ebc 88 stepq4 = stepq4*(10.0 + 1.0/9.0);
pbdt1997 1:5ed21efc1d58 89 }
pbdt1997 4:fb0905390ebc 90 }
pbdt1997 4:fb0905390ebc 91
pbdt1997 4:fb0905390ebc 92 void driveStepper(){
pbdt1997 1:5ed21efc1d58 93 if(moveq4 == true){
pbdt1997 4:fb0905390ebc 94 if(countq4 >= stepq4){
pbdt1997 4:fb0905390ebc 95 moveq4 = false;
pbdt1997 4:fb0905390ebc 96 countq4 = 0;
pbdt1997 4:fb0905390ebc 97 PUL_2 = 0;
pbdt1997 4:fb0905390ebc 98 }
pbdt1997 4:fb0905390ebc 99 else{
pbdt1997 4:fb0905390ebc 100 PUL_2 = !PUL_2;
pbdt1997 4:fb0905390ebc 101 countq4++;
pbdt1997 4:fb0905390ebc 102 }
pbdt1997 1:5ed21efc1d58 103 }
pbdt1997 1:5ed21efc1d58 104 }
pbdt1997 1:5ed21efc1d58 105
pbdt1997 0:a3b83d874092 106 void setHome(){
pbdt1997 0:a3b83d874092 107 while(prox2 == 1){
pbdt1997 0:a3b83d874092 108 drvStepper2(1);
pbdt1997 0:a3b83d874092 109 }
pbdt1997 0:a3b83d874092 110 wait(0.5);
pbdt1997 1:5ed21efc1d58 111 drvStepper2(-138.51);
pbdt1997 0:a3b83d874092 112 wait(0.5);
pbdt1997 1:5ed21efc1d58 113 countq4 = 0;
pbdt1997 1:5ed21efc1d58 114 q4 = 0;
pbdt1997 0:a3b83d874092 115 while(prox1 == 1){
pbdt1997 0:a3b83d874092 116 drvStepper1(-1);
pbdt1997 0:a3b83d874092 117 }
pbdt1997 0:a3b83d874092 118 wait(0.5);
pbdt1997 1:5ed21efc1d58 119 drvStepper1(18);
pbdt1997 0:a3b83d874092 120 wait(0.5);
pbdt1997 1:5ed21efc1d58 121 countq3 = 0;
pbdt1997 1:5ed21efc1d58 122 q3 = 0;
pbdt1997 0:a3b83d874092 123 }
pbdt1997 0:a3b83d874092 124
pbdt1997 4:fb0905390ebc 125 void stop_q3(){
pbdt1997 4:fb0905390ebc 126 PUL_1 = 0;
pbdt1997 4:fb0905390ebc 127 }
pbdt1997 4:fb0905390ebc 128
pbdt1997 4:fb0905390ebc 129 void stop_q4(){
pbdt1997 4:fb0905390ebc 130 PUL_2 = 0;
pbdt1997 4:fb0905390ebc 131 }
pbdt1997 4:fb0905390ebc 132
pbdt1997 4:fb0905390ebc 133 void veloControl4(float angle, float speed){
pbdt1997 4:fb0905390ebc 134 stepper.detach();
pbdt1997 4:fb0905390ebc 135 stepper.attach_us(&driveStepper, 1000000/(speed));
pbdt1997 4:fb0905390ebc 136 if(angle - q4 >= 0){
pbdt1997 4:fb0905390ebc 137 DR_2 = 0;
pbdt1997 4:fb0905390ebc 138 }
pbdt1997 4:fb0905390ebc 139 else{
pbdt1997 4:fb0905390ebc 140 DR_2 = 1;
pbdt1997 4:fb0905390ebc 141 }
pbdt1997 1:5ed21efc1d58 142 moveq4 = true;
pbdt1997 4:fb0905390ebc 143 convertStep(angle, 4);
pbdt1997 1:5ed21efc1d58 144 }
pbdt1997 4:fb0905390ebc 145
pbdt1997 4:fb0905390ebc 146
pbdt1997 4:fb0905390ebc 147 int main() {
pbdt1997 4:fb0905390ebc 148 stepper.attach_us(&driveStepper, 10000.0);
pbdt1997 4:fb0905390ebc 149 pc.baud(250000);
pbdt1997 4:fb0905390ebc 150 //prox1.rise(&stop_q3);
pbdt1997 4:fb0905390ebc 151 // prox2.rise(&stop_q4);
pbdt1997 4:fb0905390ebc 152 moveq4 = false;
pbdt1997 4:fb0905390ebc 153 setHome();
pbdt1997 4:fb0905390ebc 154 wait(2);
pbdt1997 4:fb0905390ebc 155 countq3 = 0;
pbdt1997 4:fb0905390ebc 156 countq4 = 0;
pbdt1997 4:fb0905390ebc 157
pbdt1997 4:fb0905390ebc 158 veloControl4(60.0, 100);
pbdt1997 4:fb0905390ebc 159 wait(5);
pbdt1997 4:fb0905390ebc 160 veloControl4(30.0, 1000);
pbdt1997 4:fb0905390ebc 161 while(1){
pbdt1997 4:fb0905390ebc 162
pbdt1997 4:fb0905390ebc 163 printf("stepq4 %.2f countq4 %.2f\n",stepq4,countq4);
pbdt1997 4:fb0905390ebc 164
pbdt1997 4:fb0905390ebc 165 }
pbdt1997 4:fb0905390ebc 166
pbdt1997 0:a3b83d874092 167 }