Marco Tulio Masselli Rainho Teixeira / Mbed 2 deprecated tracking_cam_5

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

mian.cpp@0:e687ed46d0dc, 2022-08-02 (annotated)

Committer:
marcotmrt
Date:
Tue Aug 02 17:27:08 2022 +0000
Revision:
0:e687ed46d0dc
Tracking cam

Who changed what in which revision?

UserRevisionLine numberNew contents of line
marcotmrt 0:e687ed46d0dc 1 #include "mbed.h"
marcotmrt 0:e687ed46d0dc 2
marcotmrt 0:e687ed46d0dc 3 PwmOut servo_x(D5);
marcotmrt 0:e687ed46d0dc 4 PwmOut servo_y(D6);
marcotmrt 0:e687ed46d0dc 5 DigitalOut led1(D13);
marcotmrt 0:e687ed46d0dc 6
marcotmrt 0:e687ed46d0dc 7 Serial python(D8, D2, 9600);
marcotmrt 0:e687ed46d0dc 8 Serial pc(USBTX, USBRX, 9600);
marcotmrt 0:e687ed46d0dc 9
marcotmrt 0:e687ed46d0dc 10 int MIN = 510;
marcotmrt 0:e687ed46d0dc 11 int MAX = 2350;
marcotmrt 0:e687ed46d0dc 12 int MID = (MAX + MIN)/2;
marcotmrt 0:e687ed46d0dc 13 int OFFSET = 300;
marcotmrt 0:e687ed46d0dc 14
marcotmrt 0:e687ed46d0dc 15 int STEP = 5;
marcotmrt 0:e687ed46d0dc 16 int CSTEP_x = MID;
marcotmrt 0:e687ed46d0dc 17 int CSTEP_y = MID;
marcotmrt 0:e687ed46d0dc 18
marcotmrt 0:e687ed46d0dc 19 int state = 0;
marcotmrt 0:e687ed46d0dc 20 char rx_line;
marcotmrt 0:e687ed46d0dc 21
marcotmrt 0:e687ed46d0dc 22
marcotmrt 0:e687ed46d0dc 23 void Rx_interrupt();
marcotmrt 0:e687ed46d0dc 24 void calibrate();
marcotmrt 0:e687ed46d0dc 25 void move_x(int dir, int max, int min, int step);
marcotmrt 0:e687ed46d0dc 26 void move_y(int dir, int max, int min, int step);
marcotmrt 0:e687ed46d0dc 27
marcotmrt 0:e687ed46d0dc 28
marcotmrt 0:e687ed46d0dc 29
marcotmrt 0:e687ed46d0dc 30 int main() {
marcotmrt 0:e687ed46d0dc 31
marcotmrt 0:e687ed46d0dc 32 // toda vez q chegar(Rx) info pela serial, execura a funcao interupt
marcotmrt 0:e687ed46d0dc 33 python.attach(&Rx_interrupt, Serial::RxIrq);
marcotmrt 0:e687ed46d0dc 34
marcotmrt 0:e687ed46d0dc 35 servo_x.period_us(20000); //20ms period, typical for analog RC servo
marcotmrt 0:e687ed46d0dc 36 servo_y.period_us(20000);
marcotmrt 0:e687ed46d0dc 37
marcotmrt 0:e687ed46d0dc 38 calibrate();
marcotmrt 0:e687ed46d0dc 39
marcotmrt 0:e687ed46d0dc 40 while(1){
marcotmrt 0:e687ed46d0dc 41
marcotmrt 0:e687ed46d0dc 42 if(state == 1){
marcotmrt 0:e687ed46d0dc 43 pc.printf("state = 1\n");
marcotmrt 0:e687ed46d0dc 44 wait(0.1);
marcotmrt 0:e687ed46d0dc 45 while(state == 1){
marcotmrt 0:e687ed46d0dc 46 if(CSTEP_x < (MAX-OFFSET)){
marcotmrt 0:e687ed46d0dc 47 servo_x.pulsewidth_us(CSTEP_x + STEP);
marcotmrt 0:e687ed46d0dc 48 CSTEP_x = CSTEP_x + STEP;
marcotmrt 0:e687ed46d0dc 49 wait(0.01);
marcotmrt 0:e687ed46d0dc 50 }
marcotmrt 0:e687ed46d0dc 51 }
marcotmrt 0:e687ed46d0dc 52 }
marcotmrt 0:e687ed46d0dc 53 if(state == 2){
marcotmrt 0:e687ed46d0dc 54 pc.printf("state = 2\n");
marcotmrt 0:e687ed46d0dc 55 wait(0.1);
marcotmrt 0:e687ed46d0dc 56 while(state == 2){
marcotmrt 0:e687ed46d0dc 57 if(CSTEP_x > (MIN+OFFSET)){
marcotmrt 0:e687ed46d0dc 58 servo_x.pulsewidth_us(CSTEP_x - STEP);
marcotmrt 0:e687ed46d0dc 59 CSTEP_x = CSTEP_x - STEP;
marcotmrt 0:e687ed46d0dc 60 wait(0.01);
marcotmrt 0:e687ed46d0dc 61 }
marcotmrt 0:e687ed46d0dc 62 }
marcotmrt 0:e687ed46d0dc 63 }
marcotmrt 0:e687ed46d0dc 64 if(state == 3){
marcotmrt 0:e687ed46d0dc 65 pc.printf("state = 3\n");
marcotmrt 0:e687ed46d0dc 66 wait(0.1);
marcotmrt 0:e687ed46d0dc 67 while(state == 3){
marcotmrt 0:e687ed46d0dc 68 if(CSTEP_y < (MAX-OFFSET)){
marcotmrt 0:e687ed46d0dc 69 servo_y.pulsewidth_us(CSTEP_y + STEP);
marcotmrt 0:e687ed46d0dc 70 CSTEP_y = CSTEP_y + STEP;
marcotmrt 0:e687ed46d0dc 71 wait(0.01);
marcotmrt 0:e687ed46d0dc 72 }
marcotmrt 0:e687ed46d0dc 73 }
marcotmrt 0:e687ed46d0dc 74 }
marcotmrt 0:e687ed46d0dc 75 if(state == 4){
marcotmrt 0:e687ed46d0dc 76 pc.printf("state = 4\n");
marcotmrt 0:e687ed46d0dc 77 wait(0.1);
marcotmrt 0:e687ed46d0dc 78 while(state == 4){
marcotmrt 0:e687ed46d0dc 79 if(CSTEP_y > (MIN+OFFSET)){
marcotmrt 0:e687ed46d0dc 80 servo_y.pulsewidth_us(CSTEP_y - STEP);
marcotmrt 0:e687ed46d0dc 81 CSTEP_y = CSTEP_y - STEP;
marcotmrt 0:e687ed46d0dc 82 wait(0.01);
marcotmrt 0:e687ed46d0dc 83 }
marcotmrt 0:e687ed46d0dc 84 }
marcotmrt 0:e687ed46d0dc 85 }
marcotmrt 0:e687ed46d0dc 86 if(state == 5){
marcotmrt 0:e687ed46d0dc 87 //pc.printf("state = 5\n");
marcotmrt 0:e687ed46d0dc 88 wait(1);
marcotmrt 0:e687ed46d0dc 89 //break;
marcotmrt 0:e687ed46d0dc 90 }
marcotmrt 0:e687ed46d0dc 91
marcotmrt 0:e687ed46d0dc 92 }
marcotmrt 0:e687ed46d0dc 93
marcotmrt 0:e687ed46d0dc 94
marcotmrt 0:e687ed46d0dc 95 }
marcotmrt 0:e687ed46d0dc 96
marcotmrt 0:e687ed46d0dc 97
marcotmrt 0:e687ed46d0dc 98
marcotmrt 0:e687ed46d0dc 99
marcotmrt 0:e687ed46d0dc 100
marcotmrt 0:e687ed46d0dc 101 void move_x(int dir, int max, int min, int step){
marcotmrt 0:e687ed46d0dc 102
marcotmrt 0:e687ed46d0dc 103 if(dir == 0){
marcotmrt 0:e687ed46d0dc 104 pc.printf("dir x = 0\n");
marcotmrt 0:e687ed46d0dc 105 char txt[10];
marcotmrt 0:e687ed46d0dc 106 sprintf(txt,"CSTEP_x=%d",CSTEP_x);
marcotmrt 0:e687ed46d0dc 107 pc.printf(txt);
marcotmrt 0:e687ed46d0dc 108 pc.printf("\n");
marcotmrt 0:e687ed46d0dc 109 for(int pw_x=CSTEP_x; pw_x<max; pw_x+=step){
marcotmrt 0:e687ed46d0dc 110 servo_x.pulsewidth_us(pw_x);
marcotmrt 0:e687ed46d0dc 111 CSTEP_x = pw_x;
marcotmrt 0:e687ed46d0dc 112 wait(0.01);
marcotmrt 0:e687ed46d0dc 113 }
marcotmrt 0:e687ed46d0dc 114 }
marcotmrt 0:e687ed46d0dc 115
marcotmrt 0:e687ed46d0dc 116 else if(dir == 1){
marcotmrt 0:e687ed46d0dc 117 pc.printf("dir x = 1\n");
marcotmrt 0:e687ed46d0dc 118 char txt[10];
marcotmrt 0:e687ed46d0dc 119 sprintf(txt,"CSTEP_x=%d",CSTEP_x);
marcotmrt 0:e687ed46d0dc 120 pc.printf(txt);
marcotmrt 0:e687ed46d0dc 121 pc.printf("\n");
marcotmrt 0:e687ed46d0dc 122 for(int pw_x=CSTEP_x; pw_x>min; pw_x-=step){
marcotmrt 0:e687ed46d0dc 123 servo_x.pulsewidth_us(pw_x);
marcotmrt 0:e687ed46d0dc 124 CSTEP_x = pw_x;
marcotmrt 0:e687ed46d0dc 125 wait(0.01);
marcotmrt 0:e687ed46d0dc 126 }
marcotmrt 0:e687ed46d0dc 127 }
marcotmrt 0:e687ed46d0dc 128 }
marcotmrt 0:e687ed46d0dc 129
marcotmrt 0:e687ed46d0dc 130
marcotmrt 0:e687ed46d0dc 131
marcotmrt 0:e687ed46d0dc 132
marcotmrt 0:e687ed46d0dc 133
marcotmrt 0:e687ed46d0dc 134
marcotmrt 0:e687ed46d0dc 135
marcotmrt 0:e687ed46d0dc 136 void calibrate(){
marcotmrt 0:e687ed46d0dc 137 servo_x.pulsewidth_us(MID);
marcotmrt 0:e687ed46d0dc 138 wait(0.5);
marcotmrt 0:e687ed46d0dc 139 servo_x.pulsewidth_us(MID + 460);
marcotmrt 0:e687ed46d0dc 140 wait(0.5);
marcotmrt 0:e687ed46d0dc 141 servo_x.pulsewidth_us(MID - 460);
marcotmrt 0:e687ed46d0dc 142 wait(0.5);
marcotmrt 0:e687ed46d0dc 143 servo_x.pulsewidth_us(MID);
marcotmrt 0:e687ed46d0dc 144 wait(0.5);
marcotmrt 0:e687ed46d0dc 145
marcotmrt 0:e687ed46d0dc 146 servo_y.pulsewidth_us(MID);
marcotmrt 0:e687ed46d0dc 147 wait(0.5);
marcotmrt 0:e687ed46d0dc 148 servo_y.pulsewidth_us(MID + 460);
marcotmrt 0:e687ed46d0dc 149 wait(0.5);
marcotmrt 0:e687ed46d0dc 150 servo_y.pulsewidth_us(MID - 460);
marcotmrt 0:e687ed46d0dc 151 wait(0.5);
marcotmrt 0:e687ed46d0dc 152 servo_y.pulsewidth_us(MID);
marcotmrt 0:e687ed46d0dc 153 wait(0.5);
marcotmrt 0:e687ed46d0dc 154
marcotmrt 0:e687ed46d0dc 155
marcotmrt 0:e687ed46d0dc 156
marcotmrt 0:e687ed46d0dc 157 }
marcotmrt 0:e687ed46d0dc 158
marcotmrt 0:e687ed46d0dc 159
marcotmrt 0:e687ed46d0dc 160
marcotmrt 0:e687ed46d0dc 161
marcotmrt 0:e687ed46d0dc 162 void Rx_interrupt() // funcao que recebe os outputs do script python
marcotmrt 0:e687ed46d0dc 163 {
marcotmrt 0:e687ed46d0dc 164 led1=1;
marcotmrt 0:e687ed46d0dc 165 while(python.readable())
marcotmrt 0:e687ed46d0dc 166
marcotmrt 0:e687ed46d0dc 167
marcotmrt 0:e687ed46d0dc 168 // Recebe o char do buffer usado pelo dispositivo "python"
marcotmrt 0:e687ed46d0dc 169 rx_line = python.getc();
marcotmrt 0:e687ed46d0dc 170
marcotmrt 0:e687ed46d0dc 171 switch(rx_line)
marcotmrt 0:e687ed46d0dc 172 {
marcotmrt 0:e687ed46d0dc 173
marcotmrt 0:e687ed46d0dc 174 case '1':
marcotmrt 0:e687ed46d0dc 175 state = 1;
marcotmrt 0:e687ed46d0dc 176 rx_line = 0x00;
marcotmrt 0:e687ed46d0dc 177 break;
marcotmrt 0:e687ed46d0dc 178
marcotmrt 0:e687ed46d0dc 179 case '2':
marcotmrt 0:e687ed46d0dc 180 state = 2;
marcotmrt 0:e687ed46d0dc 181 rx_line = 0x00;
marcotmrt 0:e687ed46d0dc 182 break;
marcotmrt 0:e687ed46d0dc 183
marcotmrt 0:e687ed46d0dc 184 case '3':
marcotmrt 0:e687ed46d0dc 185 state = 3;
marcotmrt 0:e687ed46d0dc 186 rx_line = 0x00;
marcotmrt 0:e687ed46d0dc 187 break;
marcotmrt 0:e687ed46d0dc 188
marcotmrt 0:e687ed46d0dc 189 case '4':
marcotmrt 0:e687ed46d0dc 190 state = 4;
marcotmrt 0:e687ed46d0dc 191 rx_line = 0x00;
marcotmrt 0:e687ed46d0dc 192 break;
marcotmrt 0:e687ed46d0dc 193
marcotmrt 0:e687ed46d0dc 194 case '5':
marcotmrt 0:e687ed46d0dc 195 state = 5;
marcotmrt 0:e687ed46d0dc 196 rx_line = 0x00;
marcotmrt 0:e687ed46d0dc 197 break;
marcotmrt 0:e687ed46d0dc 198
marcotmrt 0:e687ed46d0dc 199
marcotmrt 0:e687ed46d0dc 200
marcotmrt 0:e687ed46d0dc 201 default: rx_line=0;
marcotmrt 0:e687ed46d0dc 202 led1=0;
marcotmrt 0:e687ed46d0dc 203 }
marcotmrt 0:e687ed46d0dc 204 return;
marcotmrt 0:e687ed46d0dc 205 }