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@2:9a3221b22855, 2016-12-07 (annotated)

Committer:
syuki1021
Date:
Wed Dec 07 20:46:37 2016 +0000
Revision:
2:9a3221b22855
Parent:
1:5265e3a6f3d7
Child:
3:c9e09e968552
more changes to 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 2:9a3221b22855 24 int targets = 0;
syuki1021 1:5265e3a6f3d7 25 int TError = 0;
syuki1021 1:5265e3a6f3d7 26 char V;
syuki1021 1:5265e3a6f3d7 27 char M;
syuki1021 2:9a3221b22855 28 int current = 1;
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 2:9a3221b22855 124 for (current < targets+1){
syuki1021 1:5265e3a6f3d7 125 LMotor.speed(0);
syuki1021 1:5265e3a6f3d7 126 RMotor.speed(0);
syuki1021 1:5265e3a6f3d7 127 wait(.2);
syuki1021 1:5265e3a6f3d7 128 while(Rtot < 55 | Ltot < 55){
syuki1021 1:5265e3a6f3d7 129 LMotor.speed(.3);
syuki1021 1:5265e3a6f3d7 130 RMotor.speed(-.3);}
syuki1021 1:5265e3a6f3d7 131 LMotor.speed(0);
syuki1021 1:5265e3a6f3d7 132 RMotor.speed(0);
syuki1021 1:5265e3a6f3d7 133 pc.printf( "RTot: %d\n\r",Rtot);
syuki1021 1:5265e3a6f3d7 134 pc.printf("LTot: %d\n\r",Ltot);
syuki1021 1:5265e3a6f3d7 135 Rtot = 0;
syuki1021 1:5265e3a6f3d7 136 Ltot = 0;
syuki1021 1:5265e3a6f3d7 137 wait(15); // TAKE PICTURE
syuki1021 1:5265e3a6f3d7 138 while (V == 0){ // V = whether picture is valid or not
syuki1021 1:5265e3a6f3d7 139 wait(10);// Continue to take pictures)
syuki1021 1:5265e3a6f3d7 140 }
syuki1021 1:5265e3a6f3d7 141 if (M == 1){ // if match found drives forward
syuki1021 1:5265e3a6f3d7 142 LMotor.speed(Lspeed);
syuki1021 1:5265e3a6f3d7 143 RMotor.speed(Rspeed);
syuki1021 1:5265e3a6f3d7 144 wait(1);
syuki1021 1:5265e3a6f3d7 145 LMotor.speed(0);
syuki1021 1:5265e3a6f3d7 146 RMotor.speed(0);
syuki1021 1:5265e3a6f3d7 147 wait(1);//snap tons of photos
syuki1021 1:5265e3a6f3d7 148 LMotor.speed(-Lspeed);
syuki1021 1:5265e3a6f3d7 149 RMotor.speed(-Rspeed);
syuki1021 2:9a3221b22855 150 wait(1);
syuki1021 2:9a3221b22855 151 current = targets+1; //if a match is found, stops the search
syuki1021 1:5265e3a6f3d7 152 }
syuki1021 2:9a3221b22855 153 else current++;
syuki1021 1:5265e3a6f3d7 154 }
syuki1021 2:9a3221b22855 155 Instr = 0; //turns status to stop after search
syuki1021 1:5265e3a6f3d7 156 /*
syuki1021 0:0170bad0f358 157 switch (Instr){
syuki1021 0:0170bad0f358 158
syuki1021 0:0170bad0f358 159 // stop instruction
syuki1021 1:5265e3a6f3d7 160 case 0:
syuki1021 0:0170bad0f358 161 Rspeed = 0;
syuki1021 0:0170bad0f358 162 Lspeed = 0;
syuki1021 0:0170bad0f358 163 break;
syuki1021 0:0170bad0f358 164 // forward instruction
syuki1021 1:5265e3a6f3d7 165 case 1 : //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped.
syuki1021 0:0170bad0f358 166 Error = Lcount - Rcount;
syuki1021 0:0170bad0f358 167 //pc.printf("Lcount in sampler: %f\n\r",Lcount);
syuki1021 0:0170bad0f358 168 //pc.printf("RCount in sampler: %f\n\r",Rcount);
syuki1021 0:0170bad0f358 169 //pc.printf("Error in sampler: %f\n\r",Error);
syuki1021 0:0170bad0f358 170 //pc.printf("Rspeed: %f \n \r", Rspeed);
syuki1021 1:5265e3a6f3d7 171 Rspeed = Rspeed + ((float)Error / 10);
syuki1021 0:0170bad0f358 172 break;
syuki1021 0:0170bad0f358 173
syuki1021 0:0170bad0f358 174 // Left turn Instruction
syuki1021 1:5265e3a6f3d7 175 case 2:
syuki1021 0:0170bad0f358 176 Error = Lcount - Rcount;
syuki1021 0:0170bad0f358 177 //pc.printf("Lcount in sampler: %f\n\r",Lcount);
syuki1021 0:0170bad0f358 178 //pc.printf("RCount in sampler: %f\n\r",Rcount);
syuki1021 0:0170bad0f358 179 //pc.printf("Error in sampler: %f\n\r",Error);
syuki1021 0:0170bad0f358 180 //pc.printf("Rspeed: %f \n \r", Rspeed);
syuki1021 1:5265e3a6f3d7 181 Rspeed = Rspeed + ((float)Error / 10);
syuki1021 0:0170bad0f358 182 LMotor.speed(0);
syuki1021 0:0170bad0f358 183 RMotor.speed(0);
syuki1021 0:0170bad0f358 184 if (Lspeed >0){
syuki1021 1:5265e3a6f3d7 185 Lspeed = -Lspeed;
syuki1021 1:5265e3a6f3d7 186 }
syuki1021 0:0170bad0f358 187 break;
syuki1021 0:0170bad0f358 188
syuki1021 0:0170bad0f358 189 // Right turn Instruction
syuki1021 1:5265e3a6f3d7 190 case 3:
syuki1021 0:0170bad0f358 191 Error = Lcount - Rcount;
syuki1021 0:0170bad0f358 192 //pc.printf("Lcount in sampler: %f\n\r",Lcount);
syuki1021 0:0170bad0f358 193 //pc.printf("RCount in sampler: %f\n\r",Rcount);
syuki1021 0:0170bad0f358 194 //pc.printf("Error in sampler: %f\n\r",Error);
syuki1021 0:0170bad0f358 195 //pc.printf("Rspeed: %f \n \r", Rspeed);
syuki1021 1:5265e3a6f3d7 196 Rspeed = Rspeed + ((float)Error / 10);
syuki1021 0:0170bad0f358 197 LMotor.speed(0);
syuki1021 0:0170bad0f358 198 RMotor.speed(0);
syuki1021 0:0170bad0f358 199 if (Rspeed >0){
syuki1021 0:0170bad0f358 200 Rspeed = -Rspeed;}
syuki1021 0:0170bad0f358 201 break;
syuki1021 0:0170bad0f358 202
syuki1021 0:0170bad0f358 203 // reverse instruction
syuki1021 1:5265e3a6f3d7 204 case 4: //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped.
syuki1021 0:0170bad0f358 205 Error = Rcount - Lcount;
syuki1021 0:0170bad0f358 206 //pc.printf("Lcount in sampler: %f\n\r",Lcount);
syuki1021 0:0170bad0f358 207 //pc.printf("RCount in sampler: %f\n\r",Rcount);
syuki1021 0:0170bad0f358 208 //pc.printf("Error in sampler: %f\n\r",Error);
syuki1021 0:0170bad0f358 209 //pc.printf("Rspeed: %f \n \r", Rspeed);
syuki1021 1:5265e3a6f3d7 210 Rspeed = Rspeed + ((float)Error / 10);
syuki1021 0:0170bad0f358 211 if (Rspeed > 0){
syuki1021 0:0170bad0f358 212 Rspeed = -Rspeed;
syuki1021 0:0170bad0f358 213 }
syuki1021 0:0170bad0f358 214 if (Lspeed > 0){
syuki1021 0:0170bad0f358 215 Lspeed = -Lspeed;
syuki1021 0:0170bad0f358 216 }
syuki1021 0:0170bad0f358 217 LMotor.speed(0);
syuki1021 0:0170bad0f358 218 RMotor.speed(0);
syuki1021 0:0170bad0f358 219 break;
syuki1021 0:0170bad0f358 220 }
syuki1021 0:0170bad0f358 221
syuki1021 1:5265e3a6f3d7 222 */
syuki1021 1:5265e3a6f3d7 223 if (Rtot >10 || Ltot > 10){
syuki1021 1:5265e3a6f3d7 224 Rtot = 0;
syuki1021 1:5265e3a6f3d7 225 Ltot = 0;}
syuki1021 0:0170bad0f358 226 LMotor.speed(Lspeed);
syuki1021 0:0170bad0f358 227 RMotor.speed(Rspeed);
syuki1021 0:0170bad0f358 228 Lcount = 0; //Restart the counters
syuki1021 0:0170bad0f358 229 Rcount = 0;
syuki1021 1:5265e3a6f3d7 230
syuki1021 0:0170bad0f358 231 wait(.02);
syuki1021 0:0170bad0f358 232 }
syuki1021 1:5265e3a6f3d7 233 }