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@0:0170bad0f358, 2016-12-03 (annotated)

Committer:
syuki1021
Date:
Sat Dec 03 23:42:47 2016 +0000
Revision:
0:0170bad0f358
Child:
1:5265e3a6f3d7
Code for 4180 Final project, car control with basic encoder assistance

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 0:0170bad0f358 18 float Rspeed = .5;
syuki1021 0:0170bad0f358 19 float Lspeed = .5;
syuki1021 0:0170bad0f358 20 int Instr = 1;
syuki1021 0:0170bad0f358 21 int Speed = 50;
syuki1021 0:0170bad0f358 22 int Rtot;
syuki1021 0:0170bad0f358 23 int Ltot;
syuki1021 0:0170bad0f358 24
syuki1021 0:0170bad0f358 25 Ticker Sampler;
syuki1021 0:0170bad0f358 26
syuki1021 0:0170bad0f358 27
syuki1021 0:0170bad0f358 28 void RSample() {
syuki1021 0:0170bad0f358 29 Rcount++;
syuki1021 0:0170bad0f358 30 // Rtot = Rtot + Rcount;
syuki1021 0:0170bad0f358 31 // pc.printf("Rcount: %d\n\r",Rcount);
syuki1021 0:0170bad0f358 32 }
syuki1021 0:0170bad0f358 33
syuki1021 0:0170bad0f358 34 void LSample() {
syuki1021 0:0170bad0f358 35 Lcount++;
syuki1021 0:0170bad0f358 36 // Ltot = Ltot + Lcount;
syuki1021 0:0170bad0f358 37 }
syuki1021 0:0170bad0f358 38
syuki1021 0:0170bad0f358 39 void callback() {
syuki1021 0:0170bad0f358 40 // Note: you need to actually read from the serial to clear the RX interrupt
syuki1021 0:0170bad0f358 41 printf("%c\n",pc.getc());
syuki1021 0:0170bad0f358 42 led = !led;
syuki1021 0:0170bad0f358 43 }
syuki1021 0:0170bad0f358 44
syuki1021 0:0170bad0f358 45 int main() {
syuki1021 0:0170bad0f358 46 pc.attach(&callback);
syuki1021 0:0170bad0f358 47
syuki1021 0:0170bad0f358 48 Rencoder.mode(PullUp);
syuki1021 0:0170bad0f358 49 Rencoder.rise(&RSample);
syuki1021 0:0170bad0f358 50 Lencoder.mode(PullUp);
syuki1021 0:0170bad0f358 51 Lencoder.rise(&LSample);
syuki1021 0:0170bad0f358 52
syuki1021 0:0170bad0f358 53 while(1) {
syuki1021 0:0170bad0f358 54 switch (Instr){
syuki1021 0:0170bad0f358 55
syuki1021 0:0170bad0f358 56 // stop instruction
syuki1021 0:0170bad0f358 57 case (0):
syuki1021 0:0170bad0f358 58 Rspeed = 0;
syuki1021 0:0170bad0f358 59 Lspeed = 0;
syuki1021 0:0170bad0f358 60 break;
syuki1021 0:0170bad0f358 61 // forward instruction
syuki1021 0:0170bad0f358 62 case (1) : //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped.
syuki1021 0:0170bad0f358 63 Error = Lcount - Rcount;
syuki1021 0:0170bad0f358 64 //pc.printf("Lcount in sampler: %f\n\r",Lcount);
syuki1021 0:0170bad0f358 65 //pc.printf("RCount in sampler: %f\n\r",Rcount);
syuki1021 0:0170bad0f358 66 //pc.printf("Error in sampler: %f\n\r",Error);
syuki1021 0:0170bad0f358 67 //pc.printf("Rspeed: %f \n \r", Rspeed);
syuki1021 0:0170bad0f358 68 Rspeed = Rspeed + ((float)Error / 80);
syuki1021 0:0170bad0f358 69 break;
syuki1021 0:0170bad0f358 70
syuki1021 0:0170bad0f358 71 // Left turn Instruction
syuki1021 0:0170bad0f358 72 case (2):
syuki1021 0:0170bad0f358 73 Error = Lcount - Rcount;
syuki1021 0:0170bad0f358 74 //pc.printf("Lcount in sampler: %f\n\r",Lcount);
syuki1021 0:0170bad0f358 75 //pc.printf("RCount in sampler: %f\n\r",Rcount);
syuki1021 0:0170bad0f358 76 //pc.printf("Error in sampler: %f\n\r",Error);
syuki1021 0:0170bad0f358 77 //pc.printf("Rspeed: %f \n \r", Rspeed);
syuki1021 0:0170bad0f358 78 Rspeed = Rspeed + ((float)Error / 80);
syuki1021 0:0170bad0f358 79 LMotor.speed(0);
syuki1021 0:0170bad0f358 80 RMotor.speed(0);
syuki1021 0:0170bad0f358 81 if (Lspeed >0){
syuki1021 0:0170bad0f358 82 Lspeed = -Lspeed;}
syuki1021 0:0170bad0f358 83 break;
syuki1021 0:0170bad0f358 84
syuki1021 0:0170bad0f358 85 // Right turn Instruction
syuki1021 0:0170bad0f358 86 case (3):
syuki1021 0:0170bad0f358 87 Error = Lcount - Rcount;
syuki1021 0:0170bad0f358 88 //pc.printf("Lcount in sampler: %f\n\r",Lcount);
syuki1021 0:0170bad0f358 89 //pc.printf("RCount in sampler: %f\n\r",Rcount);
syuki1021 0:0170bad0f358 90 //pc.printf("Error in sampler: %f\n\r",Error);
syuki1021 0:0170bad0f358 91 //pc.printf("Rspeed: %f \n \r", Rspeed);
syuki1021 0:0170bad0f358 92 Rspeed = Rspeed + ((float)Error / 80);
syuki1021 0:0170bad0f358 93 LMotor.speed(0);
syuki1021 0:0170bad0f358 94 RMotor.speed(0);
syuki1021 0:0170bad0f358 95 if (Rspeed >0){
syuki1021 0:0170bad0f358 96 Rspeed = -Rspeed;}
syuki1021 0:0170bad0f358 97 break;
syuki1021 0:0170bad0f358 98
syuki1021 0:0170bad0f358 99 // reverse instruction
syuki1021 0:0170bad0f358 100 case (4): //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped.
syuki1021 0:0170bad0f358 101 Error = Rcount - Lcount;
syuki1021 0:0170bad0f358 102 //pc.printf("Lcount in sampler: %f\n\r",Lcount);
syuki1021 0:0170bad0f358 103 //pc.printf("RCount in sampler: %f\n\r",Rcount);
syuki1021 0:0170bad0f358 104 //pc.printf("Error in sampler: %f\n\r",Error);
syuki1021 0:0170bad0f358 105 //pc.printf("Rspeed: %f \n \r", Rspeed);
syuki1021 0:0170bad0f358 106 Rspeed = Rspeed + ((float)Error / 80);
syuki1021 0:0170bad0f358 107 if (Rspeed > 0){
syuki1021 0:0170bad0f358 108 Rspeed = -Rspeed;
syuki1021 0:0170bad0f358 109 }
syuki1021 0:0170bad0f358 110 if (Lspeed > 0){
syuki1021 0:0170bad0f358 111 Lspeed = -Lspeed;
syuki1021 0:0170bad0f358 112 }
syuki1021 0:0170bad0f358 113 LMotor.speed(0);
syuki1021 0:0170bad0f358 114 RMotor.speed(0);
syuki1021 0:0170bad0f358 115 break;
syuki1021 0:0170bad0f358 116 }
syuki1021 0:0170bad0f358 117
syuki1021 0:0170bad0f358 118 LMotor.speed(Lspeed);
syuki1021 0:0170bad0f358 119 RMotor.speed(Rspeed);
syuki1021 0:0170bad0f358 120 Lcount = 0; //Restart the counters
syuki1021 0:0170bad0f358 121 Rcount = 0;
syuki1021 0:0170bad0f358 122 wait(.02);
syuki1021 0:0170bad0f358 123 }
syuki1021 0:0170bad0f358 124 }
syuki1021 0:0170bad0f358 125