Grupo T / Mbed OS GRUPOT

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.

main.cpp@7:27ea14e58788, 2018-05-04 (annotated)

Committer:
lucasfontenla
Date:
Fri May 04 11:41:31 2018 +0000
Revision:
7:27ea14e58788
Parent:
6:af32c68ff4cf
Child:
8:de64672c240f
Cria??o da fun??o "distance()" que calcula a dist?ncia percorrida para um certo n?mero de passos. Ainda n?o funcionando 100%. ; Est? com algum erro em que a dist?ncia calculada ? maior que a real.;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lucasfontenla 0:cc562f78f889 1 #include "mbed.h"
lucasfontenla 0:cc562f78f889 2
lucasfontenla 5:0cbf491f703d 3 // MACHINE SETUP ---------------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 4
lucasfontenla 5:0cbf491f703d 5 // drivers output signal
lucasfontenla 4:48717b42eab2 6 DigitalOut stepX(D2);
lucasfontenla 4:48717b42eab2 7 DigitalOut dirX(D3);
lucasfontenla 4:48717b42eab2 8
lucasfontenla 4:48717b42eab2 9 DigitalOut stepY(D8);
lucasfontenla 4:48717b42eab2 10 DigitalOut dirY(D9);
lucasfontenla 4:48717b42eab2 11
lucasfontenla 4:48717b42eab2 12 DigitalOut stepZ(D11);
lucasfontenla 4:48717b42eab2 13 DigitalOut dirZ(D12);
lucasfontenla 4:48717b42eab2 14
lucasfontenla 5:0cbf491f703d 15 // hardware input signal
lucasfontenla 5:0cbf491f703d 16 // end-of-stroke sensors
lucasfontenla 5:0cbf491f703d 17 DigitalIn endX1(D5);
lucasfontenla 5:0cbf491f703d 18 DigitalIn endX2(D10);
lucasfontenla 5:0cbf491f703d 19
lucasfontenla 5:0cbf491f703d 20 // IHM for development
lucasfontenla 4:48717b42eab2 21 AnalogIn joyX(A0);
lucasfontenla 4:48717b42eab2 22 AnalogIn joyY(A1);
lucasfontenla 4:48717b42eab2 23
lucasfontenla 4:48717b42eab2 24 DigitalIn zUp(D6);
lucasfontenla 4:48717b42eab2 25 DigitalIn zDwn(D7);
lucasfontenla 4:48717b42eab2 26
lucasfontenla 5:0cbf491f703d 27 // variables definition
lucasfontenla 5:0cbf491f703d 28 int modeStatus = 1;
lucasfontenla 5:0cbf491f703d 29
lucasfontenla 4:48717b42eab2 30 float valX;
lucasfontenla 4:48717b42eab2 31 float valY;
lucasfontenla 4:48717b42eab2 32 int valZUp;
lucasfontenla 4:48717b42eab2 33 int valZDwn;
lucasfontenla 4:48717b42eab2 34
lucasfontenla 4:48717b42eab2 35 int activeX = 1;
lucasfontenla 4:48717b42eab2 36 int activeY = 1;
lucasfontenla 4:48717b42eab2 37 int activeZ = 1;
lucasfontenla 4:48717b42eab2 38
lucasfontenla 5:0cbf491f703d 39 int x_dir;
lucasfontenla 5:0cbf491f703d 40 int y_dir;
lucasfontenla 5:0cbf491f703d 41 int z_dir;
lucasfontenla 5:0cbf491f703d 42
lucasfontenla 5:0cbf491f703d 43 float ppsMax = 600.0;
lucasfontenla 5:0cbf491f703d 44
lucasfontenla 7:27ea14e58788 45 int totalX = 0;
lucasfontenla 7:27ea14e58788 46 int totalY = 0;
lucasfontenla 7:27ea14e58788 47
lucasfontenla 7:27ea14e58788 48 float X = 0;
lucasfontenla 7:27ea14e58788 49 float Y = 0;
lucasfontenla 7:27ea14e58788 50
lucasfontenla 7:27ea14e58788 51 // machine parameters
lucasfontenla 7:27ea14e58788 52 float xPitch = 5.0;
lucasfontenla 7:27ea14e58788 53 float yPitch = 5.0;
lucasfontenla 7:27ea14e58788 54
lucasfontenla 7:27ea14e58788 55 float motor_steps_by_rotation = 200.0;
lucasfontenla 7:27ea14e58788 56 float step_mode = 0.5;
lucasfontenla 7:27ea14e58788 57 float steps_by_rotation = motor_steps_by_rotation/step_mode;
lucasfontenla 7:27ea14e58788 58
lucasfontenla 7:27ea14e58788 59 // prototype functions
lucasfontenla 5:0cbf491f703d 60 void move(int pps, int x_dir, int y_dir, int z_dir, int x_step, int y_step, int z_step);
lucasfontenla 7:27ea14e58788 61 float distance(int steps, float pitch);
lucasfontenla 7:27ea14e58788 62
lucasfontenla 7:27ea14e58788 63 // MAIN PROGRAM ----------------------------------------------------------------
lucasfontenla 7:27ea14e58788 64
lucasfontenla 7:27ea14e58788 65 int contador = 0;
lucasfontenla 1:b2e395e50a45 66
lucasfontenla 1:b2e395e50a45 67 int main(){
lucasfontenla 4:48717b42eab2 68 printf("Starting...\r\n");
lucasfontenla 5:0cbf491f703d 69
lucasfontenla 5:0cbf491f703d 70 if(modeStatus){
lucasfontenla 5:0cbf491f703d 71 //Código de JOG
lucasfontenla 7:27ea14e58788 72 printf("JOG Selected\n");
lucasfontenla 5:0cbf491f703d 73 while(1){
lucasfontenla 5:0cbf491f703d 74 valX = joyX;
lucasfontenla 5:0cbf491f703d 75 valY = joyY;
lucasfontenla 5:0cbf491f703d 76
lucasfontenla 5:0cbf491f703d 77 valZUp = zUp;
lucasfontenla 5:0cbf491f703d 78 valZDwn = zDwn;
lucasfontenla 5:0cbf491f703d 79
lucasfontenla 5:0cbf491f703d 80 if(valX > 0.7){
lucasfontenla 5:0cbf491f703d 81 if(x_dir){
lucasfontenla 7:27ea14e58788 82 x_dir = 0;
lucasfontenla 5:0cbf491f703d 83 }
lucasfontenla 7:27ea14e58788 84 activeX = 1;
lucasfontenla 7:27ea14e58788 85 totalX-=1;
lucasfontenla 5:0cbf491f703d 86 }
lucasfontenla 5:0cbf491f703d 87 else if(valX < 0.3){
lucasfontenla 5:0cbf491f703d 88 if(!x_dir){
lucasfontenla 7:27ea14e58788 89 x_dir = 1;
lucasfontenla 5:0cbf491f703d 90 }
lucasfontenla 7:27ea14e58788 91 activeX = 1;
lucasfontenla 7:27ea14e58788 92 totalX+=1;
lucasfontenla 5:0cbf491f703d 93 }
lucasfontenla 5:0cbf491f703d 94 else{
lucasfontenla 5:0cbf491f703d 95 activeX = 0;
lucasfontenla 4:48717b42eab2 96 }
lucasfontenla 5:0cbf491f703d 97
lucasfontenla 5:0cbf491f703d 98 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 99
lucasfontenla 5:0cbf491f703d 100 if(valY > 0.7){
lucasfontenla 5:0cbf491f703d 101 if(!y_dir){
lucasfontenla 7:27ea14e58788 102 y_dir = 1;
lucasfontenla 5:0cbf491f703d 103 }
lucasfontenla 7:27ea14e58788 104 activeY = 1;
lucasfontenla 7:27ea14e58788 105 totalY+=1;
lucasfontenla 5:0cbf491f703d 106 }
lucasfontenla 5:0cbf491f703d 107 else if(valY < 0.3){
lucasfontenla 5:0cbf491f703d 108 if(y_dir){
lucasfontenla 5:0cbf491f703d 109 y_dir = 0;
lucasfontenla 5:0cbf491f703d 110 }
lucasfontenla 7:27ea14e58788 111 activeY = 1;
lucasfontenla 7:27ea14e58788 112 totalY-=1;
lucasfontenla 5:0cbf491f703d 113 }
lucasfontenla 5:0cbf491f703d 114 else{
lucasfontenla 5:0cbf491f703d 115 activeY = 0;
lucasfontenla 4:48717b42eab2 116 }
lucasfontenla 5:0cbf491f703d 117
lucasfontenla 5:0cbf491f703d 118 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 119
lucasfontenla 5:0cbf491f703d 120 if(!valZUp && valZDwn){
lucasfontenla 5:0cbf491f703d 121 if(z_dir){
lucasfontenla 7:27ea14e58788 122 z_dir = 0;
lucasfontenla 7:27ea14e58788 123 //totalZ-=1;
lucasfontenla 5:0cbf491f703d 124 }
lucasfontenla 5:0cbf491f703d 125 activeZ = 1;
lucasfontenla 5:0cbf491f703d 126 }
lucasfontenla 5:0cbf491f703d 127 else if(!valZDwn && valZUp){
lucasfontenla 5:0cbf491f703d 128 if(!z_dir){
lucasfontenla 7:27ea14e58788 129 z_dir = 1;
lucasfontenla 7:27ea14e58788 130 //totalZ+=1;
lucasfontenla 5:0cbf491f703d 131 }
lucasfontenla 5:0cbf491f703d 132 activeZ = 1;
lucasfontenla 5:0cbf491f703d 133 }
lucasfontenla 5:0cbf491f703d 134 else {
lucasfontenla 5:0cbf491f703d 135 activeZ = 0;
lucasfontenla 5:0cbf491f703d 136 }
lucasfontenla 5:0cbf491f703d 137
lucasfontenla 5:0cbf491f703d 138 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 139
lucasfontenla 7:27ea14e58788 140 move(ppsMax, x_dir, y_dir, z_dir, activeX, activeY, activeZ);
lucasfontenla 7:27ea14e58788 141 X = distance(totalX, xPitch);
lucasfontenla 7:27ea14e58788 142 Y = distance(totalY, yPitch);
lucasfontenla 7:27ea14e58788 143
lucasfontenla 7:27ea14e58788 144 if(contador > 10000){
lucasfontenla 7:27ea14e58788 145 X = distance(totalX, xPitch);
lucasfontenla 7:27ea14e58788 146 Y = distance(totalY, yPitch);
lucasfontenla 7:27ea14e58788 147 printf("X: %.2f Y: %.2f\r\n", X, Y);
lucasfontenla 7:27ea14e58788 148 contador = 0;
lucasfontenla 7:27ea14e58788 149 }
lucasfontenla 7:27ea14e58788 150
lucasfontenla 7:27ea14e58788 151 contador+=1;
lucasfontenla 4:48717b42eab2 152 }
lucasfontenla 5:0cbf491f703d 153 }
lucasfontenla 5:0cbf491f703d 154
lucasfontenla 5:0cbf491f703d 155 else {
lucasfontenla 5:0cbf491f703d 156 //Código de execução fora do JOG
lucasfontenla 5:0cbf491f703d 157 }
lucasfontenla 5:0cbf491f703d 158
lucasfontenla 5:0cbf491f703d 159 }
lucasfontenla 5:0cbf491f703d 160
lucasfontenla 7:27ea14e58788 161 // FUNCTIONS ----------------------------------------------------------------
lucasfontenla 7:27ea14e58788 162
lucasfontenla 5:0cbf491f703d 163 void move(int pps, int x_dir, int y_dir, int z_dir, int x_step, int y_step, int z_step){
lucasfontenla 5:0cbf491f703d 164 float time = 1.0/pps/2.0;
lucasfontenla 5:0cbf491f703d 165
lucasfontenla 5:0cbf491f703d 166 dirX = x_dir;
lucasfontenla 5:0cbf491f703d 167 dirY = y_dir;
lucasfontenla 5:0cbf491f703d 168 dirZ = z_dir;
lucasfontenla 5:0cbf491f703d 169
lucasfontenla 5:0cbf491f703d 170 int max_val;
lucasfontenla 5:0cbf491f703d 171
lucasfontenla 6:af32c68ff4cf 172 // maior número de passos -------
lucasfontenla 5:0cbf491f703d 173 if(x_step > y_step){
lucasfontenla 5:0cbf491f703d 174 if(x_step > z_step){
lucasfontenla 5:0cbf491f703d 175 max_val = x_step;
lucasfontenla 4:48717b42eab2 176 }
lucasfontenla 4:48717b42eab2 177 else {
lucasfontenla 5:0cbf491f703d 178 max_val = z_step;
lucasfontenla 4:48717b42eab2 179 }
lucasfontenla 5:0cbf491f703d 180 } else if(y_step > z_step){
lucasfontenla 5:0cbf491f703d 181 max_val = y_step;
lucasfontenla 5:0cbf491f703d 182 }
lucasfontenla 5:0cbf491f703d 183 else {
lucasfontenla 5:0cbf491f703d 184 max_val = z_step;
lucasfontenla 5:0cbf491f703d 185 }
lucasfontenla 5:0cbf491f703d 186
lucasfontenla 5:0cbf491f703d 187 for(int i = 0; i < max_val; i++){
lucasfontenla 6:af32c68ff4cf 188 if(i >= x_step){
lucasfontenla 5:0cbf491f703d 189 stepX = 0;
lucasfontenla 5:0cbf491f703d 190 }
lucasfontenla 5:0cbf491f703d 191 else { stepX = 1; }
lucasfontenla 4:48717b42eab2 192
lucasfontenla 6:af32c68ff4cf 193 if(i >= y_step){
lucasfontenla 5:0cbf491f703d 194 stepY = 0;
lucasfontenla 5:0cbf491f703d 195 }
lucasfontenla 5:0cbf491f703d 196 else { stepY = 1; }
lucasfontenla 5:0cbf491f703d 197
lucasfontenla 6:af32c68ff4cf 198 if(i >= z_step){
lucasfontenla 5:0cbf491f703d 199 stepZ = 0;
lucasfontenla 5:0cbf491f703d 200 }
lucasfontenla 5:0cbf491f703d 201 else { stepZ = 1; }
lucasfontenla 5:0cbf491f703d 202
lucasfontenla 5:0cbf491f703d 203 wait(time);
lucasfontenla 4:48717b42eab2 204 stepX = 0;
lucasfontenla 4:48717b42eab2 205 stepY = 0;
lucasfontenla 4:48717b42eab2 206 stepZ = 0;
lucasfontenla 5:0cbf491f703d 207 wait(time);
lucasfontenla 4:48717b42eab2 208 }
lucasfontenla 5:0cbf491f703d 209 }
lucasfontenla 7:27ea14e58788 210
lucasfontenla 7:27ea14e58788 211 float distance(int steps, float pitch){
lucasfontenla 7:27ea14e58788 212 float delta_S = (((float)steps)/steps_by_rotation)*pitch;
lucasfontenla 7:27ea14e58788 213 return delta_S;
lucasfontenla 7:27ea14e58788 214 }