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@5:d73ca4500060, 2016-12-10 (annotated)

Committer:
syuki1021
Date:
Sat Dec 10 22:22:31 2016 +0000
Revision:
5:d73ca4500060
Parent:
4:295160ab06c7
Child:
6:91445dd6d89a
ECE4180 paparazzi bot final version;

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 3:c9e09e968552 20 float TRspeed = .35;
syuki1021 3:c9e09e968552 21 float TLspeed = .35;
syuki1021 3:c9e09e968552 22
syuki1021 3:c9e09e968552 23 int Instr = 0;
syuki1021 1:5265e3a6f3d7 24 int Rtot=0;
syuki1021 1:5265e3a6f3d7 25 int Ltot=0;
syuki1021 1:5265e3a6f3d7 26 char c;
syuki1021 2:9a3221b22855 27 int targets = 0;
syuki1021 3:c9e09e968552 28 float TError = 0;
syuki1021 1:5265e3a6f3d7 29 char V;
syuki1021 1:5265e3a6f3d7 30 char M;
syuki1021 2:9a3221b22855 31 int current = 1;
syuki1021 0:0170bad0f358 32
syuki1021 3:c9e09e968552 33
syuki1021 0:0170bad0f358 34 Ticker Sampler;
syuki1021 0:0170bad0f358 35
syuki1021 0:0170bad0f358 36
syuki1021 0:0170bad0f358 37 void RSample() {
syuki1021 1:5265e3a6f3d7 38 Rtot++;
syuki1021 0:0170bad0f358 39 // pc.printf("Rcount: %d\n\r",Rcount);
syuki1021 0:0170bad0f358 40 }
syuki1021 0:0170bad0f358 41
syuki1021 0:0170bad0f358 42 void LSample() {
syuki1021 1:5265e3a6f3d7 43 Ltot++;
syuki1021 0:0170bad0f358 44 }
syuki1021 0:0170bad0f358 45
syuki1021 0:0170bad0f358 46 void callback() {
syuki1021 1:5265e3a6f3d7 47 c = pc.getc();
syuki1021 3:c9e09e968552 48 // printf("%c",pic);
syuki1021 3:c9e09e968552 49 // pc.printf("%c",pic);
syuki1021 3:c9e09e968552 50 //pc.printf("Hello world2 \n \r");
syuki1021 0:0170bad0f358 51 led = !led;
syuki1021 1:5265e3a6f3d7 52 if (c == '0') { //Stop
syuki1021 1:5265e3a6f3d7 53 Instr = 0;
syuki1021 4:295160ab06c7 54 } else if (c == '1') { //reverse
syuki1021 1:5265e3a6f3d7 55 Instr = 1;
syuki1021 3:c9e09e968552 56 } else if (c == '2') { //Left
syuki1021 1:5265e3a6f3d7 57 Instr = 2;
syuki1021 1:5265e3a6f3d7 58 } else if (c == '3') { //Right
syuki1021 1:5265e3a6f3d7 59 Instr = 3;
syuki1021 4:295160ab06c7 60 } else if (c == '4') { //forward
syuki1021 1:5265e3a6f3d7 61 Instr = 4;
syuki1021 3:c9e09e968552 62 } else if (c == 'A') { //Rotate + take pic
syuki1021 3:c9e09e968552 63 Instr = 5;
syuki1021 3:c9e09e968552 64 } else if (c == 'D') { //move foward set amount
syuki1021 5:d73ca4500060 65 Instr = 6;}
syuki1021 0:0170bad0f358 66 }
syuki1021 0:0170bad0f358 67
syuki1021 0:0170bad0f358 68 int main() {
syuki1021 0:0170bad0f358 69 pc.attach(&callback);
syuki1021 0:0170bad0f358 70
syuki1021 0:0170bad0f358 71 Rencoder.mode(PullUp);
syuki1021 0:0170bad0f358 72 Rencoder.rise(&RSample);
syuki1021 0:0170bad0f358 73 Lencoder.mode(PullUp);
syuki1021 0:0170bad0f358 74 Lencoder.rise(&LSample);
syuki1021 0:0170bad0f358 75
syuki1021 3:c9e09e968552 76 while(1) {
syuki1021 1:5265e3a6f3d7 77
syuki1021 1:5265e3a6f3d7 78 //printf("Instr = %d\n", Instr);
syuki1021 1:5265e3a6f3d7 79
syuki1021 1:5265e3a6f3d7 80 if (Instr == 0) {
syuki1021 1:5265e3a6f3d7 81 Rspeed = 0;
syuki1021 1:5265e3a6f3d7 82 Lspeed = 0;
syuki1021 3:c9e09e968552 83 } else if (Instr == 1) { //move backwards (forward, we turned our bot around)
syuki1021 4:295160ab06c7 84 Rspeed = .4;
syuki1021 4:295160ab06c7 85 Lspeed = .4;
syuki1021 1:5265e3a6f3d7 86 TError = Ltot - Rtot;
syuki1021 4:295160ab06c7 87 TError = TError +.05;
syuki1021 4:295160ab06c7 88 Rspeed = fabs(Rspeed) + (float)TError /100;
syuki1021 4:295160ab06c7 89
syuki1021 3:c9e09e968552 90 if(Rspeed < 0)
syuki1021 3:c9e09e968552 91 RMotor.speed(0);
syuki1021 3:c9e09e968552 92 if(Lspeed < 0)
syuki1021 3:c9e09e968552 93 LMotor.speed(0); //sets speed to 0 because motor cannot go from negative to positive without hitting 0
syuki1021 3:c9e09e968552 94
syuki1021 3:c9e09e968552 95 } else if (Instr == 2) { //turn Left
syuki1021 1:5265e3a6f3d7 96 TError = Ltot - Rtot;
syuki1021 4:295160ab06c7 97 if(TError > 0) TRspeed = fabs(TRspeed)+.01;
syuki1021 3:c9e09e968552 98 else if (TError == 0) TRspeed = TRspeed;
syuki1021 4:295160ab06c7 99 else TRspeed = fabs(TRspeed) - .01;
syuki1021 3:c9e09e968552 100 // Rspeed = Rspeed + .0008;
syuki1021 1:5265e3a6f3d7 101 LMotor.speed(0);
syuki1021 1:5265e3a6f3d7 102 RMotor.speed(0);
syuki1021 4:295160ab06c7 103 TLspeed = -fabs(TLspeed); //Lwheel must move in reverse for left turn
syuki1021 4:295160ab06c7 104 TRspeed = fabs(TRspeed); //Rwheel must move forward for left turn
syuki1021 1:5265e3a6f3d7 105
syuki1021 3:c9e09e968552 106 } else if (Instr == 3) { // turn right
syuki1021 1:5265e3a6f3d7 107 TError = Ltot - Rtot;
syuki1021 4:295160ab06c7 108 if(TError > 0) TRspeed = fabs(TRspeed)+.01;
syuki1021 3:c9e09e968552 109 else if (TError == 0) TRspeed = TRspeed;
syuki1021 4:295160ab06c7 110 else TRspeed = fabs(TRspeed) - .01;
syuki1021 3:c9e09e968552 111 // Rspeed = Rspeed + .0008;
syuki1021 1:5265e3a6f3d7 112 LMotor.speed(0);
syuki1021 1:5265e3a6f3d7 113 RMotor.speed(0);
syuki1021 4:295160ab06c7 114 TRspeed = -fabs(TRspeed);
syuki1021 4:295160ab06c7 115 TLspeed = fabs(TLspeed);
syuki1021 3:c9e09e968552 116 } else if (Instr == 4) { // go forward (reverse, we turned our bot around)
syuki1021 4:295160ab06c7 117 Rspeed = .4;
syuki1021 4:295160ab06c7 118 Lspeed = .4;
syuki1021 3:c9e09e968552 119 TError = Ltot - Rtot;
syuki1021 4:295160ab06c7 120 Rspeed = fabs(Rspeed) + (float) TError / 100;
syuki1021 4:295160ab06c7 121 Rspeed = -fabs(Rspeed);
syuki1021 4:295160ab06c7 122 Lspeed = -fabs(Lspeed);
syuki1021 1:5265e3a6f3d7 123 LMotor.speed(0);
syuki1021 1:5265e3a6f3d7 124 RMotor.speed(0);
syuki1021 3:c9e09e968552 125 }
syuki1021 3:c9e09e968552 126 else if (Instr == 5) { //Rotate + Pic
syuki1021 1:5265e3a6f3d7 127
syuki1021 3:c9e09e968552 128
syuki1021 1:5265e3a6f3d7 129 LMotor.speed(0);
syuki1021 1:5265e3a6f3d7 130 RMotor.speed(0);
syuki1021 1:5265e3a6f3d7 131 wait(.2);
syuki1021 3:c9e09e968552 132 while(Rtot < 50 | Ltot < 50){
syuki1021 3:c9e09e968552 133 LMotor.speed(.4);
syuki1021 3:c9e09e968552 134 RMotor.speed(-.4);}
syuki1021 1:5265e3a6f3d7 135 LMotor.speed(0);
syuki1021 1:5265e3a6f3d7 136 RMotor.speed(0);
syuki1021 1:5265e3a6f3d7 137 Rtot = 0;
syuki1021 1:5265e3a6f3d7 138 Ltot = 0;
syuki1021 3:c9e09e968552 139 wait(1);
syuki1021 3:c9e09e968552 140 pc.printf("P\n"); //commands pi to take pic
syuki1021 2:9a3221b22855 141 Instr = 0; //turns status to stop after search
syuki1021 1:5265e3a6f3d7 142 }
syuki1021 5:d73ca4500060 143 else if (Instr == 6) { //code when match found
syuki1021 5:d73ca4500060 144
syuki1021 5:d73ca4500060 145 LMotor.speed(0);
syuki1021 5:d73ca4500060 146 RMotor.speed(0);
syuki1021 5:d73ca4500060 147 Ltot = 0;
syuki1021 5:d73ca4500060 148 Rtot = 0;
syuki1021 5:d73ca4500060 149 TError = 0;
syuki1021 5:d73ca4500060 150 while(Ltot < 200)
syuki1021 5:d73ca4500060 151 LMotor.speed(.4);
syuki1021 5:d73ca4500060 152 LMotor.speed(0);
syuki1021 5:d73ca4500060 153 wait(1);
syuki1021 5:d73ca4500060 154 Ltot = 0;
syuki1021 5:d73ca4500060 155 while(Ltot < 200)
syuki1021 5:d73ca4500060 156 LMotor.speed(-.4);
syuki1021 5:d73ca4500060 157 LMotor.speed(0);
syuki1021 5:d73ca4500060 158 wait(.5);
syuki1021 5:d73ca4500060 159 while(Rtot < 200)
syuki1021 5:d73ca4500060 160 RMotor.speed(.4);
syuki1021 5:d73ca4500060 161 RMotor.speed(0);
syuki1021 5:d73ca4500060 162 wait(.5);
syuki1021 5:d73ca4500060 163 Rtot = 0;
syuki1021 5:d73ca4500060 164 while(Rtot < 200)
syuki1021 5:d73ca4500060 165 RMotor.speed(-.4);
syuki1021 5:d73ca4500060 166 RMotor.speed(0);
syuki1021 5:d73ca4500060 167 wait(.5);
syuki1021 5:d73ca4500060 168 Ltot = 0;
syuki1021 5:d73ca4500060 169 Rtot = 0;
syuki1021 5:d73ca4500060 170 while((Ltot +Rtot) < 2000){
syuki1021 5:d73ca4500060 171 RMotor.speed(.4);
syuki1021 5:d73ca4500060 172 LMotor.speed(-.4);}
syuki1021 5:d73ca4500060 173 RMotor.speed(0);
syuki1021 5:d73ca4500060 174 LMotor.speed(0);
syuki1021 5:d73ca4500060 175 Instr=0;
syuki1021 5:d73ca4500060 176 }
syuki1021 3:c9e09e968552 177
syuki1021 0:0170bad0f358 178
syuki1021 1:5265e3a6f3d7 179 if (Rtot >10 || Ltot > 10){
syuki1021 1:5265e3a6f3d7 180 Rtot = 0;
syuki1021 1:5265e3a6f3d7 181 Ltot = 0;}
syuki1021 3:c9e09e968552 182
syuki1021 3:c9e09e968552 183 if (Instr == 2 | Instr == 3){
syuki1021 3:c9e09e968552 184 LMotor.speed(TLspeed);
syuki1021 3:c9e09e968552 185 RMotor.speed(TRspeed);}
syuki1021 3:c9e09e968552 186 else{
syuki1021 3:c9e09e968552 187 LMotor.speed(Lspeed);
syuki1021 3:c9e09e968552 188 RMotor.speed(Rspeed);}
syuki1021 4:295160ab06c7 189
syuki1021 0:0170bad0f358 190 Lcount = 0; //Restart the counters
syuki1021 0:0170bad0f358 191 Rcount = 0;
syuki1021 1:5265e3a6f3d7 192
syuki1021 0:0170bad0f358 193 wait(.02);
syuki1021 0:0170bad0f358 194 }
syuki1021 1:5265e3a6f3d7 195 }