satoshi yuki / Mbed 2 deprecated mbed_paparazzibot_4180Final

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:   Motor mbed

main.cpp@1:5265e3a6f3d7, 2016-12-07 (annotated)

Committer:
syuki1021
Date:
Wed Dec 07 20:36:22 2016 +0000
Revision:
1:5265e3a6f3d7
Parent:
0:0170bad0f358
Child:
2:9a3221b22855
more changes to improve code;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
syuki1021 0:0170bad0f358 1 #include "mbed.h"
syuki1021 0:0170bad0f358 2 #include "Motor.h"
syuki1021 0:0170bad0f358 3 Serial pc(USBTX, USBRX);
syuki1021 0:0170bad0f358 4
syuki1021 0:0170bad0f358 5
syuki1021 0:0170bad0f358 6 InterruptIn Rencoder(p30);
syuki1021 0:0170bad0f358 7 InterruptIn Lencoder(p29);
syuki1021 0:0170bad0f358 8
syuki1021 0:0170bad0f358 9 DigitalOut led(LED1);
syuki1021 0:0170bad0f358 10 DigitalOut flash(LED4);
syuki1021 0:0170bad0f358 11
syuki1021 0:0170bad0f358 12 Motor LMotor(p22, p23, p24); // pwmA, fwd, rev,
syuki1021 0:0170bad0f358 13 Motor RMotor(p21, p26, p25); // pwmB, fwd, rev,
syuki1021 0:0170bad0f358 14
syuki1021 0:0170bad0f358 15 int Rcount = 0;
syuki1021 0:0170bad0f358 16 int Lcount = 0;
syuki1021 0:0170bad0f358 17 int Error = 0;
syuki1021 1:5265e3a6f3d7 18 float Rspeed = .4;
syuki1021 1:5265e3a6f3d7 19 float Lspeed = .4;
syuki1021 1:5265e3a6f3d7 20 int Instr = 5;
syuki1021 1:5265e3a6f3d7 21 int Rtot=0;
syuki1021 1:5265e3a6f3d7 22 int Ltot=0;
syuki1021 1:5265e3a6f3d7 23 char c;
syuki1021 1:5265e3a6f3d7 24 int pics = 0;
syuki1021 1:5265e3a6f3d7 25 int TError = 0;
syuki1021 1:5265e3a6f3d7 26 char V;
syuki1021 1:5265e3a6f3d7 27 char M;
syuki1021 1:5265e3a6f3d7 28
syuki1021 0:0170bad0f358 29
syuki1021 0:0170bad0f358 30 Ticker Sampler;
syuki1021 0:0170bad0f358 31
syuki1021 0:0170bad0f358 32
syuki1021 0:0170bad0f358 33 void RSample() {
syuki1021 1:5265e3a6f3d7 34 Rtot++;
syuki1021 0:0170bad0f358 35 // pc.printf("Rcount: %d\n\r",Rcount);
syuki1021 0:0170bad0f358 36 }
syuki1021 0:0170bad0f358 37
syuki1021 0:0170bad0f358 38 void LSample() {
syuki1021 1:5265e3a6f3d7 39 Ltot++;
syuki1021 0:0170bad0f358 40 }
syuki1021 0:0170bad0f358 41
syuki1021 0:0170bad0f358 42 void callback() {
syuki1021 0:0170bad0f358 43 // Note: you need to actually read from the serial to clear the RX interrupt
syuki1021 1:5265e3a6f3d7 44 //printf("%c\n",pc.getc());
syuki1021 1:5265e3a6f3d7 45 //printf("Hello world");
syuki1021 1:5265e3a6f3d7 46 c = pc.getc();
syuki1021 0:0170bad0f358 47 led = !led;
syuki1021 1:5265e3a6f3d7 48 if (c == '0') { //Stop
syuki1021 1:5265e3a6f3d7 49 Instr = 0;
syuki1021 1:5265e3a6f3d7 50 } else if (c == '1') { //Forward
syuki1021 1:5265e3a6f3d7 51 Instr = 1;
syuki1021 1:5265e3a6f3d7 52 }else if (c == '2') { //Left
syuki1021 1:5265e3a6f3d7 53 Instr = 2;
syuki1021 1:5265e3a6f3d7 54 } else if (c == '3') { //Right
syuki1021 1:5265e3a6f3d7 55 Instr = 3;
syuki1021 1:5265e3a6f3d7 56 } else if (c == '4') { //REverse
syuki1021 1:5265e3a6f3d7 57 Instr = 4;
syuki1021 1:5265e3a6f3d7 58 }
syuki1021 0:0170bad0f358 59 }
syuki1021 0:0170bad0f358 60
syuki1021 0:0170bad0f358 61 int main() {
syuki1021 0:0170bad0f358 62 pc.attach(&callback);
syuki1021 0:0170bad0f358 63
syuki1021 0:0170bad0f358 64 Rencoder.mode(PullUp);
syuki1021 0:0170bad0f358 65 Rencoder.rise(&RSample);
syuki1021 0:0170bad0f358 66 Lencoder.mode(PullUp);
syuki1021 0:0170bad0f358 67 Lencoder.rise(&LSample);
syuki1021 0:0170bad0f358 68
syuki1021 0:0170bad0f358 69 while(1) {
syuki1021 1:5265e3a6f3d7 70
syuki1021 1:5265e3a6f3d7 71 //printf("Instr = %d\n", Instr);
syuki1021 1:5265e3a6f3d7 72
syuki1021 1:5265e3a6f3d7 73 if (Instr == 0) {
syuki1021 1:5265e3a6f3d7 74 Rspeed = 0;
syuki1021 1:5265e3a6f3d7 75 Lspeed = 0;
syuki1021 1:5265e3a6f3d7 76 } else if (Instr == 1) {
syuki1021 1:5265e3a6f3d7 77 TError = Ltot - Rtot;
syuki1021 1:5265e3a6f3d7 78 if(TError > 0) Rspeed = Rspeed+.01;
syuki1021 1:5265e3a6f3d7 79 else if (TError == 0) Rspeed = Rspeed;
syuki1021 1:5265e3a6f3d7 80 else Rspeed = Rspeed - .01;
syuki1021 1:5265e3a6f3d7 81 Rspeed = Rspeed + .0008;
syuki1021 1:5265e3a6f3d7 82 } else if (Instr == 2) {
syuki1021 1:5265e3a6f3d7 83 TError = Ltot - Rtot;
syuki1021 1:5265e3a6f3d7 84 if(TError > 0) Rspeed = Rspeed+.01;
syuki1021 1:5265e3a6f3d7 85 else if (TError == 0) Rspeed = Rspeed;
syuki1021 1:5265e3a6f3d7 86 else Rspeed = Rspeed - .01;
syuki1021 1:5265e3a6f3d7 87 Rspeed = Rspeed + .0008;
syuki1021 1:5265e3a6f3d7 88 LMotor.speed(0);
syuki1021 1:5265e3a6f3d7 89 RMotor.speed(0);
syuki1021 1:5265e3a6f3d7 90 if (Lspeed >0){
syuki1021 1:5265e3a6f3d7 91 Lspeed = -Lspeed;
syuki1021 1:5265e3a6f3d7 92 }
syuki1021 1:5265e3a6f3d7 93
syuki1021 1:5265e3a6f3d7 94
syuki1021 1:5265e3a6f3d7 95 } else if (Instr == 3) {
syuki1021 1:5265e3a6f3d7 96 TError = Ltot - Rtot;
syuki1021 1:5265e3a6f3d7 97 if(TError > 0) Rspeed = abs(Rspeed)+.01;
syuki1021 1:5265e3a6f3d7 98 else if (TError == 0) Rspeed = Rspeed;
syuki1021 1:5265e3a6f3d7 99 else Rspeed = abs(Rspeed) - .01;
syuki1021 1:5265e3a6f3d7 100 Rspeed = Rspeed + .0008;
syuki1021 1:5265e3a6f3d7 101 LMotor.speed(0);
syuki1021 1:5265e3a6f3d7 102 RMotor.speed(0);
syuki1021 1:5265e3a6f3d7 103 if (Rspeed >0){
syuki1021 1:5265e3a6f3d7 104 Rspeed = -Rspeed;
syuki1021 1:5265e3a6f3d7 105 }
syuki1021 1:5265e3a6f3d7 106
syuki1021 1:5265e3a6f3d7 107 } else if (Instr == 4) {
syuki1021 1:5265e3a6f3d7 108 TError = Ltot - Rtot;
syuki1021 1:5265e3a6f3d7 109 if(TError > 0) Rspeed = abs(Rspeed)+.0075;
syuki1021 1:5265e3a6f3d7 110 else if (TError == 0) Rspeed = Rspeed;
syuki1021 1:5265e3a6f3d7 111 else Rspeed = abs(Rspeed) - .005;
syuki1021 1:5265e3a6f3d7 112 Rspeed = Rspeed + .00075;
syuki1021 1:5265e3a6f3d7 113 if (Rspeed > 0){
syuki1021 1:5265e3a6f3d7 114 Rspeed = -Rspeed;
syuki1021 1:5265e3a6f3d7 115 }
syuki1021 1:5265e3a6f3d7 116 if (Lspeed > 0){
syuki1021 1:5265e3a6f3d7 117 Lspeed = -Lspeed;
syuki1021 1:5265e3a6f3d7 118 }
syuki1021 1:5265e3a6f3d7 119 LMotor.speed(0);
syuki1021 1:5265e3a6f3d7 120 RMotor.speed(0);
syuki1021 1:5265e3a6f3d7 121 }
syuki1021 1:5265e3a6f3d7 122 else if (Instr == 5) {
syuki1021 1:5265e3a6f3d7 123
syuki1021 1:5265e3a6f3d7 124 LMotor.speed(0);
syuki1021 1:5265e3a6f3d7 125 RMotor.speed(0);
syuki1021 1:5265e3a6f3d7 126 wait(.2);
syuki1021 1:5265e3a6f3d7 127 while(Rtot < 55 | Ltot < 55){
syuki1021 1:5265e3a6f3d7 128 LMotor.speed(.3);
syuki1021 1:5265e3a6f3d7 129 RMotor.speed(-.3);}
syuki1021 1:5265e3a6f3d7 130 LMotor.speed(0);
syuki1021 1:5265e3a6f3d7 131 RMotor.speed(0);
syuki1021 1:5265e3a6f3d7 132 pc.printf( "RTot: %d\n\r",Rtot);
syuki1021 1:5265e3a6f3d7 133 pc.printf("LTot: %d\n\r",Ltot);
syuki1021 1:5265e3a6f3d7 134 Rtot = 0;
syuki1021 1:5265e3a6f3d7 135 Ltot = 0;
syuki1021 1:5265e3a6f3d7 136 wait(15); // TAKE PICTURE
syuki1021 1:5265e3a6f3d7 137 while (V == 0){ // V = whether picture is valid or not
syuki1021 1:5265e3a6f3d7 138 wait(10);// Continue to take pictures)
syuki1021 1:5265e3a6f3d7 139 }
syuki1021 1:5265e3a6f3d7 140 if (M == 1){ // if match found drives forward
syuki1021 1:5265e3a6f3d7 141 LMotor.speed(Lspeed);
syuki1021 1:5265e3a6f3d7 142 RMotor.speed(Rspeed);
syuki1021 1:5265e3a6f3d7 143 wait(1);
syuki1021 1:5265e3a6f3d7 144 LMotor.speed(0);
syuki1021 1:5265e3a6f3d7 145 RMotor.speed(0);
syuki1021 1:5265e3a6f3d7 146 wait(1);//snap tons of photos
syuki1021 1:5265e3a6f3d7 147 LMotor.speed(-Lspeed);
syuki1021 1:5265e3a6f3d7 148 RMotor.speed(-Rspeed);
syuki1021 1:5265e3a6f3d7 149 }
syuki1021 1:5265e3a6f3d7 150 }
syuki1021 1:5265e3a6f3d7 151 /*
syuki1021 0:0170bad0f358 152 switch (Instr){
syuki1021 0:0170bad0f358 153
syuki1021 0:0170bad0f358 154 // stop instruction
syuki1021 1:5265e3a6f3d7 155 case 0:
syuki1021 0:0170bad0f358 156 Rspeed = 0;
syuki1021 0:0170bad0f358 157 Lspeed = 0;
syuki1021 0:0170bad0f358 158 break;
syuki1021 0:0170bad0f358 159 // forward instruction
syuki1021 1:5265e3a6f3d7 160 case 1 : //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped.
syuki1021 0:0170bad0f358 161 Error = Lcount - Rcount;
syuki1021 0:0170bad0f358 162 //pc.printf("Lcount in sampler: %f\n\r",Lcount);
syuki1021 0:0170bad0f358 163 //pc.printf("RCount in sampler: %f\n\r",Rcount);
syuki1021 0:0170bad0f358 164 //pc.printf("Error in sampler: %f\n\r",Error);
syuki1021 0:0170bad0f358 165 //pc.printf("Rspeed: %f \n \r", Rspeed);
syuki1021 1:5265e3a6f3d7 166 Rspeed = Rspeed + ((float)Error / 10);
syuki1021 0:0170bad0f358 167 break;
syuki1021 0:0170bad0f358 168
syuki1021 0:0170bad0f358 169 // Left turn Instruction
syuki1021 1:5265e3a6f3d7 170 case 2:
syuki1021 0:0170bad0f358 171 Error = Lcount - Rcount;
syuki1021 0:0170bad0f358 172 //pc.printf("Lcount in sampler: %f\n\r",Lcount);
syuki1021 0:0170bad0f358 173 //pc.printf("RCount in sampler: %f\n\r",Rcount);
syuki1021 0:0170bad0f358 174 //pc.printf("Error in sampler: %f\n\r",Error);
syuki1021 0:0170bad0f358 175 //pc.printf("Rspeed: %f \n \r", Rspeed);
syuki1021 1:5265e3a6f3d7 176 Rspeed = Rspeed + ((float)Error / 10);
syuki1021 0:0170bad0f358 177 LMotor.speed(0);
syuki1021 0:0170bad0f358 178 RMotor.speed(0);
syuki1021 0:0170bad0f358 179 if (Lspeed >0){
syuki1021 1:5265e3a6f3d7 180 Lspeed = -Lspeed;
syuki1021 1:5265e3a6f3d7 181 }
syuki1021 0:0170bad0f358 182 break;
syuki1021 0:0170bad0f358 183
syuki1021 0:0170bad0f358 184 // Right turn Instruction
syuki1021 1:5265e3a6f3d7 185 case 3:
syuki1021 0:0170bad0f358 186 Error = Lcount - Rcount;
syuki1021 0:0170bad0f358 187 //pc.printf("Lcount in sampler: %f\n\r",Lcount);
syuki1021 0:0170bad0f358 188 //pc.printf("RCount in sampler: %f\n\r",Rcount);
syuki1021 0:0170bad0f358 189 //pc.printf("Error in sampler: %f\n\r",Error);
syuki1021 0:0170bad0f358 190 //pc.printf("Rspeed: %f \n \r", Rspeed);
syuki1021 1:5265e3a6f3d7 191 Rspeed = Rspeed + ((float)Error / 10);
syuki1021 0:0170bad0f358 192 LMotor.speed(0);
syuki1021 0:0170bad0f358 193 RMotor.speed(0);
syuki1021 0:0170bad0f358 194 if (Rspeed >0){
syuki1021 0:0170bad0f358 195 Rspeed = -Rspeed;}
syuki1021 0:0170bad0f358 196 break;
syuki1021 0:0170bad0f358 197
syuki1021 0:0170bad0f358 198 // reverse instruction
syuki1021 1:5265e3a6f3d7 199 case 4: //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped.
syuki1021 0:0170bad0f358 200 Error = Rcount - Lcount;
syuki1021 0:0170bad0f358 201 //pc.printf("Lcount in sampler: %f\n\r",Lcount);
syuki1021 0:0170bad0f358 202 //pc.printf("RCount in sampler: %f\n\r",Rcount);
syuki1021 0:0170bad0f358 203 //pc.printf("Error in sampler: %f\n\r",Error);
syuki1021 0:0170bad0f358 204 //pc.printf("Rspeed: %f \n \r", Rspeed);
syuki1021 1:5265e3a6f3d7 205 Rspeed = Rspeed + ((float)Error / 10);
syuki1021 0:0170bad0f358 206 if (Rspeed > 0){
syuki1021 0:0170bad0f358 207 Rspeed = -Rspeed;
syuki1021 0:0170bad0f358 208 }
syuki1021 0:0170bad0f358 209 if (Lspeed > 0){
syuki1021 0:0170bad0f358 210 Lspeed = -Lspeed;
syuki1021 0:0170bad0f358 211 }
syuki1021 0:0170bad0f358 212 LMotor.speed(0);
syuki1021 0:0170bad0f358 213 RMotor.speed(0);
syuki1021 0:0170bad0f358 214 break;
syuki1021 0:0170bad0f358 215 }
syuki1021 0:0170bad0f358 216
syuki1021 1:5265e3a6f3d7 217 */
syuki1021 1:5265e3a6f3d7 218 if (Rtot >10 || Ltot > 10){
syuki1021 1:5265e3a6f3d7 219 Rtot = 0;
syuki1021 1:5265e3a6f3d7 220 Ltot = 0;}
syuki1021 0:0170bad0f358 221 LMotor.speed(Lspeed);
syuki1021 0:0170bad0f358 222 RMotor.speed(Rspeed);
syuki1021 0:0170bad0f358 223 Lcount = 0; //Restart the counters
syuki1021 0:0170bad0f358 224 Rcount = 0;
syuki1021 1:5265e3a6f3d7 225
syuki1021 0:0170bad0f358 226 wait(.02);
syuki1021 0:0170bad0f358 227 }
syuki1021 1:5265e3a6f3d7 228 }