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

Committer:
syuki1021
Date:
Sat Dec 10 20:20:07 2016 +0000
Revision:
3:c9e09e968552
Parent:
2:9a3221b22855
Child:
4:295160ab06c7
final version maybe?;

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 3:c9e09e968552 54 } else if (c == '4') { //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 3:c9e09e968552 60 } else if (c == '1') { //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 3:c9e09e968552 65 ;}
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 1:5265e3a6f3d7 84 TError = Ltot - Rtot;
syuki1021 3:c9e09e968552 85 TError = (float) TError +.05;
syuki1021 3:c9e09e968552 86 Rspeed = abs(Rspeed) + (float)TError /100;
syuki1021 3:c9e09e968552 87 Lspeed = abs(Lspeed);
syuki1021 3:c9e09e968552 88 if(Rspeed < 0)
syuki1021 3:c9e09e968552 89 RMotor.speed(0);
syuki1021 3:c9e09e968552 90 if(Lspeed < 0)
syuki1021 3:c9e09e968552 91 LMotor.speed(0); //sets speed to 0 because motor cannot go from negative to positive without hitting 0
syuki1021 3:c9e09e968552 92
syuki1021 3:c9e09e968552 93 } else if (Instr == 2) { //turn Left
syuki1021 1:5265e3a6f3d7 94 TError = Ltot - Rtot;
syuki1021 3:c9e09e968552 95 if(TError > 0) TRspeed = abs(TRspeed)+.01;
syuki1021 3:c9e09e968552 96 else if (TError == 0) TRspeed = TRspeed;
syuki1021 3:c9e09e968552 97 else TRspeed = abs(TRspeed) - .01;
syuki1021 3:c9e09e968552 98 // Rspeed = Rspeed + .0008;
syuki1021 1:5265e3a6f3d7 99 LMotor.speed(0);
syuki1021 1:5265e3a6f3d7 100 RMotor.speed(0);
syuki1021 3:c9e09e968552 101 TLspeed = -abs(TLspeed); //Lwheel must move in reverse for left turn
syuki1021 3:c9e09e968552 102 TRspeed = abs(TRspeed); //Rwheel must move forward for left turn
syuki1021 1:5265e3a6f3d7 103
syuki1021 3:c9e09e968552 104 } else if (Instr == 3) { // turn right
syuki1021 1:5265e3a6f3d7 105 TError = Ltot - Rtot;
syuki1021 3:c9e09e968552 106 if(TError > 0) TRspeed = abs(TRspeed)+.01;
syuki1021 3:c9e09e968552 107 else if (TError == 0) TRspeed = TRspeed;
syuki1021 3:c9e09e968552 108 else TRspeed = abs(TRspeed) - .01;
syuki1021 3:c9e09e968552 109 // Rspeed = Rspeed + .0008;
syuki1021 1:5265e3a6f3d7 110 LMotor.speed(0);
syuki1021 1:5265e3a6f3d7 111 RMotor.speed(0);
syuki1021 3:c9e09e968552 112 TRspeed = -abs(TRspeed);
syuki1021 3:c9e09e968552 113 TLspeed = abs(TLspeed);
syuki1021 3:c9e09e968552 114 } else if (Instr == 4) { // go forward (reverse, we turned our bot around)
syuki1021 3:c9e09e968552 115 TError = Ltot - Rtot;
syuki1021 3:c9e09e968552 116 Rspeed = abs(Rspeed) + (float) TError / 100;
syuki1021 3:c9e09e968552 117 Rspeed = -abs(Rspeed);
syuki1021 3:c9e09e968552 118 Lspeed = -abs(Lspeed);
syuki1021 1:5265e3a6f3d7 119 LMotor.speed(0);
syuki1021 1:5265e3a6f3d7 120 RMotor.speed(0);
syuki1021 3:c9e09e968552 121 }
syuki1021 3:c9e09e968552 122 else if (Instr == 5) { //Rotate + Pic
syuki1021 1:5265e3a6f3d7 123
syuki1021 3:c9e09e968552 124
syuki1021 1:5265e3a6f3d7 125 LMotor.speed(0);
syuki1021 1:5265e3a6f3d7 126 RMotor.speed(0);
syuki1021 1:5265e3a6f3d7 127 wait(.2);
syuki1021 3:c9e09e968552 128 while(Rtot < 50 | Ltot < 50){
syuki1021 3:c9e09e968552 129 LMotor.speed(.4);
syuki1021 3:c9e09e968552 130 RMotor.speed(-.4);}
syuki1021 1:5265e3a6f3d7 131 LMotor.speed(0);
syuki1021 1:5265e3a6f3d7 132 RMotor.speed(0);
syuki1021 1:5265e3a6f3d7 133 Rtot = 0;
syuki1021 1:5265e3a6f3d7 134 Ltot = 0;
syuki1021 3:c9e09e968552 135 wait(1);
syuki1021 3:c9e09e968552 136 pc.printf("P\n"); //commands pi to take pic
syuki1021 2:9a3221b22855 137 Instr = 0; //turns status to stop after search
syuki1021 1:5265e3a6f3d7 138 }
syuki1021 3:c9e09e968552 139 else if (Instr == 6) { //code for nudging forward
syuki1021 3:c9e09e968552 140 ; }
syuki1021 3:c9e09e968552 141
syuki1021 0:0170bad0f358 142
syuki1021 1:5265e3a6f3d7 143 if (Rtot >10 || Ltot > 10){
syuki1021 1:5265e3a6f3d7 144 Rtot = 0;
syuki1021 1:5265e3a6f3d7 145 Ltot = 0;}
syuki1021 3:c9e09e968552 146
syuki1021 3:c9e09e968552 147 if (Instr == 2 | Instr == 3){
syuki1021 3:c9e09e968552 148 LMotor.speed(TLspeed);
syuki1021 3:c9e09e968552 149 RMotor.speed(TRspeed);}
syuki1021 3:c9e09e968552 150 else{
syuki1021 3:c9e09e968552 151 LMotor.speed(Lspeed);
syuki1021 3:c9e09e968552 152 RMotor.speed(Rspeed);}
syuki1021 0:0170bad0f358 153 Lcount = 0; //Restart the counters
syuki1021 0:0170bad0f358 154 Rcount = 0;
syuki1021 1:5265e3a6f3d7 155
syuki1021 0:0170bad0f358 156 wait(.02);
syuki1021 0:0170bad0f358 157 }
syuki1021 1:5265e3a6f3d7 158 }