Nishant Shah / Mbed 2 deprecated 4180-Project-Chaser

Chaser for 4180 Project

Dependencies:   Motor Servo mbed

main.cpp@0:5e63106c5452, 2016-12-12 (annotated)

Committer:
nini1294
Date:
Mon Dec 12 03:41:15 2016 +0000
Revision:
0:5e63106c5452
Initial Commit;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nini1294 0:5e63106c5452 1 #include "mbed.h"
nini1294 0:5e63106c5452 2 #include "Motor.h"
nini1294 0:5e63106c5452 3 #include "Servo.h"
nini1294 0:5e63106c5452 4 #include "string"
nini1294 0:5e63106c5452 5
nini1294 0:5e63106c5452 6 #define SERVO_MOVE 0.005
nini1294 0:5e63106c5452 7
nini1294 0:5e63106c5452 8 DigitalOut myled(LED1);
nini1294 0:5e63106c5452 9 AnalogIn light(p18);
nini1294 0:5e63106c5452 10
nini1294 0:5e63106c5452 11 RawSerial pc(USBTX, USBRX);
nini1294 0:5e63106c5452 12 RawSerial dev(p13,p14);
nini1294 0:5e63106c5452 13 DigitalOut l1(p11);
nini1294 0:5e63106c5452 14 DigitalOut l2(p12);
nini1294 0:5e63106c5452 15 DigitalOut l3(p10);
nini1294 0:5e63106c5452 16 DigitalOut led1(LED1);
nini1294 0:5e63106c5452 17 //DigitalOut led4(LED4);
nini1294 0:5e63106c5452 18
nini1294 0:5e63106c5452 19 Motor m1(p21, p5, p6);
nini1294 0:5e63106c5452 20 Motor m2(p22, p7, p8);
nini1294 0:5e63106c5452 21
nini1294 0:5e63106c5452 22 Servo horiz(p24);
nini1294 0:5e63106c5452 23 Servo vert(p23);
nini1294 0:5e63106c5452 24
nini1294 0:5e63106c5452 25 Timeout flipper;
nini1294 0:5e63106c5452 26
nini1294 0:5e63106c5452 27 void flip() {
nini1294 0:5e63106c5452 28 printf("Fire off\n");
nini1294 0:5e63106c5452 29 l1 = 0;
nini1294 0:5e63106c5452 30 l2 = 0;
nini1294 0:5e63106c5452 31 l3 = 0;
nini1294 0:5e63106c5452 32 }
nini1294 0:5e63106c5452 33
nini1294 0:5e63106c5452 34 void dev_recv()
nini1294 0:5e63106c5452 35 {
nini1294 0:5e63106c5452 36 led1 = !led1;
nini1294 0:5e63106c5452 37 while(dev.readable()) {
nini1294 0:5e63106c5452 38 switch(dev.getc()) {
nini1294 0:5e63106c5452 39 case 'm': {
nini1294 0:5e63106c5452 40 char xbuf[6];
nini1294 0:5e63106c5452 41 char ybuf[6];
nini1294 0:5e63106c5452 42
nini1294 0:5e63106c5452 43 int ind = 0;
nini1294 0:5e63106c5452 44 char cur = dev.getc();
nini1294 0:5e63106c5452 45 while(cur != ',') {
nini1294 0:5e63106c5452 46 xbuf[ind++] = cur;
nini1294 0:5e63106c5452 47 cur = dev.getc();
nini1294 0:5e63106c5452 48 }
nini1294 0:5e63106c5452 49 xbuf[ind] = NULL;
nini1294 0:5e63106c5452 50
nini1294 0:5e63106c5452 51 ind = 0;
nini1294 0:5e63106c5452 52 cur = dev.getc();
nini1294 0:5e63106c5452 53 while(cur != '|') {
nini1294 0:5e63106c5452 54 ybuf[ind++] = cur;
nini1294 0:5e63106c5452 55 cur = dev.getc();
nini1294 0:5e63106c5452 56 }
nini1294 0:5e63106c5452 57 ybuf[ind] = NULL;
nini1294 0:5e63106c5452 58
nini1294 0:5e63106c5452 59 float x, y;
nini1294 0:5e63106c5452 60 x = (float)atof(xbuf);
nini1294 0:5e63106c5452 61 y = (float)atof(ybuf);
nini1294 0:5e63106c5452 62
nini1294 0:5e63106c5452 63 pc.printf("%f, %f\n",x , y);
nini1294 0:5e63106c5452 64
nini1294 0:5e63106c5452 65 if (x >= 0.1) {
nini1294 0:5e63106c5452 66 m1.speed( (y + x) );
nini1294 0:5e63106c5452 67 m2.speed(y);
nini1294 0:5e63106c5452 68 // pc.printf("GREATER THAN %f, %f\n",(y + x) , y);
nini1294 0:5e63106c5452 69 } else if (x <= -0.1) {
nini1294 0:5e63106c5452 70 m1.speed(y);
nini1294 0:5e63106c5452 71 m2.speed( (y - x));
nini1294 0:5e63106c5452 72 // pc.printf("LESS THAN %f, %f\n",y, (y - x));
nini1294 0:5e63106c5452 73 } else {
nini1294 0:5e63106c5452 74 m1.speed(y);
nini1294 0:5e63106c5452 75 m2.speed(y);
nini1294 0:5e63106c5452 76 // pc.printf("%.2f, %.2f\n",x, y);
nini1294 0:5e63106c5452 77 }
nini1294 0:5e63106c5452 78 }
nini1294 0:5e63106c5452 79 case 's': {
nini1294 0:5e63106c5452 80 switch(dev.getc()) {
nini1294 0:5e63106c5452 81 case 'u': {
nini1294 0:5e63106c5452 82 if (vert < 1) {
nini1294 0:5e63106c5452 83 vert = vert + SERVO_MOVE;
nini1294 0:5e63106c5452 84 printf("Servo Command: %f\n", vert.read());
nini1294 0:5e63106c5452 85 }
nini1294 0:5e63106c5452 86 break;
nini1294 0:5e63106c5452 87 }
nini1294 0:5e63106c5452 88 case 'd': {
nini1294 0:5e63106c5452 89 if (vert > 0) {
nini1294 0:5e63106c5452 90 vert = vert - SERVO_MOVE;
nini1294 0:5e63106c5452 91 printf("Servo Command: %f\n", vert.read());
nini1294 0:5e63106c5452 92 }
nini1294 0:5e63106c5452 93 break;
nini1294 0:5e63106c5452 94 }
nini1294 0:5e63106c5452 95 case 'l': {
nini1294 0:5e63106c5452 96 if (horiz < 5) {
nini1294 0:5e63106c5452 97 horiz = horiz + SERVO_MOVE;
nini1294 0:5e63106c5452 98 }
nini1294 0:5e63106c5452 99 break;
nini1294 0:5e63106c5452 100 }
nini1294 0:5e63106c5452 101 case 'r': {
nini1294 0:5e63106c5452 102 if (horiz > 0) {
nini1294 0:5e63106c5452 103 horiz = horiz - SERVO_MOVE;
nini1294 0:5e63106c5452 104 }
nini1294 0:5e63106c5452 105 break;
nini1294 0:5e63106c5452 106 }
nini1294 0:5e63106c5452 107 }
nini1294 0:5e63106c5452 108 break;
nini1294 0:5e63106c5452 109 }
nini1294 0:5e63106c5452 110 case 'f': {
nini1294 0:5e63106c5452 111 printf("Fire\n");
nini1294 0:5e63106c5452 112 l1 = 1;
nini1294 0:5e63106c5452 113 l2 = 1;
nini1294 0:5e63106c5452 114 l3 = 1;
nini1294 0:5e63106c5452 115 flipper.attach(&flip, 60.0);
nini1294 0:5e63106c5452 116 break;
nini1294 0:5e63106c5452 117 }
nini1294 0:5e63106c5452 118 }
nini1294 0:5e63106c5452 119 }
nini1294 0:5e63106c5452 120 }
nini1294 0:5e63106c5452 121
nini1294 0:5e63106c5452 122 void pc_recv()
nini1294 0:5e63106c5452 123 {
nini1294 0:5e63106c5452 124 // led4 = !led4;
nini1294 0:5e63106c5452 125 while(pc.readable()) {
nini1294 0:5e63106c5452 126 dev.putc(pc.getc());
nini1294 0:5e63106c5452 127 }
nini1294 0:5e63106c5452 128 }
nini1294 0:5e63106c5452 129
nini1294 0:5e63106c5452 130 int main()
nini1294 0:5e63106c5452 131 {
nini1294 0:5e63106c5452 132 pc.baud(9600);
nini1294 0:5e63106c5452 133 dev.baud(9600);
nini1294 0:5e63106c5452 134
nini1294 0:5e63106c5452 135
nini1294 0:5e63106c5452 136 pc.attach(&pc_recv, Serial::RxIrq);
nini1294 0:5e63106c5452 137 dev.attach(&dev_recv, Serial::RxIrq);
nini1294 0:5e63106c5452 138 horiz = 0.5;
nini1294 0:5e63106c5452 139 vert = 0.5;
nini1294 0:5e63106c5452 140 while(1) {
nini1294 0:5e63106c5452 141 sleep();
nini1294 0:5e63106c5452 142 }
nini1294 0:5e63106c5452 143 }