Polybot / Mbed 2 deprecated Polulu_TP_II_ET5

tp1

Dependencies:   mbed

main.cpp@1:236ac79b07d7, 2020-10-11 (annotated)

Committer:
Maximousse
Date:
Sun Oct 11 12:58:03 2020 +0000
Revision:
1:236ac79b07d7
Parent:
0:727643d2c6ef 
commit;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Maximousse 0:727643d2c6ef 1 #include "mbed.h"
Maximousse 0:727643d2c6ef 2 #include "m3pi.h"
Maximousse 0:727643d2c6ef 3
Maximousse 0:727643d2c6ef 4 #define BUFF_SIZE 200
Maximousse 0:727643d2c6ef 5
Maximousse 0:727643d2c6ef 6
Maximousse 0:727643d2c6ef 7 Serial pc(USBTX, USBRX);
Maximousse 0:727643d2c6ef 8 DigitalOut myled(LED1);
Maximousse 0:727643d2c6ef 9 DigitalOut m (LED4);
Maximousse 0:727643d2c6ef 10 InterruptIn Msg(USBRX,PullUp);
Maximousse 0:727643d2c6ef 11 m3pi pi;
Maximousse 0:727643d2c6ef 12 int flag;
Maximousse 0:727643d2c6ef 13 char clavier;
Maximousse 0:727643d2c6ef 14 char buffer[BUFF_SIZE];
Maximousse 0:727643d2c6ef 15
Maximousse 0:727643d2c6ef 16 void readBattery()
Maximousse 0:727643d2c6ef 17 {
Maximousse 0:727643d2c6ef 18 char s[500];
Maximousse 0:727643d2c6ef 19 char* s2 = "Battery:%f V\n";
Maximousse 0:727643d2c6ef 20 sprintf(s, s2, pi.battery());
Maximousse 0:727643d2c6ef 21 for (int i=0; i<strlen(s); i++) {
Maximousse 0:727643d2c6ef 22 pc.putc(s[i]);
Maximousse 0:727643d2c6ef 23 }
Maximousse 0:727643d2c6ef 24 pc.printf("\r");
Maximousse 0:727643d2c6ef 25 }
Maximousse 0:727643d2c6ef 26
Maximousse 0:727643d2c6ef 27 void readSensors()
Maximousse 0:727643d2c6ef 28 {
Maximousse 0:727643d2c6ef 29 char s[500];
Maximousse 0:727643d2c6ef 30 char* s2 = "Sensors:(%u,%u,%u,%u,%u) \n";
Maximousse 0:727643d2c6ef 31 unsigned short val[5];
Maximousse 0:727643d2c6ef 32 pi.calibrated_sensors(val);
Maximousse 0:727643d2c6ef 33 sprintf(s, s2, val[0],val[1],val[2],val[3],val[4]);
Maximousse 0:727643d2c6ef 34 for (int i=0; i<strlen(s); i++) {
Maximousse 0:727643d2c6ef 35 pc.putc(s[i]);
Maximousse 0:727643d2c6ef 36 }
Maximousse 0:727643d2c6ef 37 pc.printf("\r");
Maximousse 0:727643d2c6ef 38 }
Maximousse 0:727643d2c6ef 39
Maximousse 0:727643d2c6ef 40 void Interrupt()
Maximousse 0:727643d2c6ef 41 {
Maximousse 0:727643d2c6ef 42 //printf("%c",pc.getc());
Maximousse 0:727643d2c6ef 43 //Command(pc.getc());
Maximousse 0:727643d2c6ef 44 flag=1;
Maximousse 0:727643d2c6ef 45 m=1;
Maximousse 0:727643d2c6ef 46 }
Maximousse 0:727643d2c6ef 47
Maximousse 0:727643d2c6ef 48 void Command(char c){
Maximousse 0:727643d2c6ef 49 switch (c) {
Maximousse 0:727643d2c6ef 50 case 'z':
Maximousse 0:727643d2c6ef 51 pi.forward(0.5);
Maximousse 0:727643d2c6ef 52 wait(0.5);
Maximousse 0:727643d2c6ef 53 pi.stop();
Maximousse 0:727643d2c6ef 54 break;
Maximousse 0:727643d2c6ef 55 case 'q':
Maximousse 0:727643d2c6ef 56 pi.left(0.5);
Maximousse 0:727643d2c6ef 57 wait(0.5);
Maximousse 0:727643d2c6ef 58 pi.stop();
Maximousse 0:727643d2c6ef 59 break;
Maximousse 0:727643d2c6ef 60 case 's':
Maximousse 0:727643d2c6ef 61 pi.backward(0.5);
Maximousse 0:727643d2c6ef 62 wait(0.5);
Maximousse 0:727643d2c6ef 63 pi.stop();
Maximousse 0:727643d2c6ef 64 break;
Maximousse 0:727643d2c6ef 65 case 'd':
Maximousse 0:727643d2c6ef 66 pi.right(0.5);
Maximousse 0:727643d2c6ef 67 wait(0.5);
Maximousse 0:727643d2c6ef 68 pi.stop();
Maximousse 0:727643d2c6ef 69 break;
Maximousse 0:727643d2c6ef 70 case 'b':
Maximousse 0:727643d2c6ef 71 readBattery();
Maximousse 0:727643d2c6ef 72 break;
Maximousse 0:727643d2c6ef 73 case 'c':
Maximousse 0:727643d2c6ef 74 readSensors();
Maximousse 0:727643d2c6ef 75 break;
Maximousse 0:727643d2c6ef 76 }
Maximousse 0:727643d2c6ef 77 }
Maximousse 0:727643d2c6ef 78
Maximousse 0:727643d2c6ef 79 int main()
Maximousse 0:727643d2c6ef 80 {
Maximousse 1:236ac79b07d7 81 pc.attach(&Interrupt);
Maximousse 0:727643d2c6ef 82 pc.baud(9600);
Maximousse 0:727643d2c6ef 83 pi.sensor_auto_calibrate();
Maximousse 0:727643d2c6ef 84 //clavier='a';
Maximousse 0:727643d2c6ef 85
Maximousse 0:727643d2c6ef 86 //flag = 0;
Maximousse 0:727643d2c6ef 87 while(1) {
Maximousse 0:727643d2c6ef 88 ///BLINK LEDS
Maximousse 0:727643d2c6ef 89 myled = 1;
Maximousse 0:727643d2c6ef 90 wait(0.1);
Maximousse 0:727643d2c6ef 91 myled = 0;
Maximousse 0:727643d2c6ef 92 wait(0.1);
Maximousse 0:727643d2c6ef 93
Maximousse 0:727643d2c6ef 94 ///GET INPUT
Maximousse 0:727643d2c6ef 95 int i = 0;
Maximousse 0:727643d2c6ef 96 while (pc.readable()) {
Maximousse 0:727643d2c6ef 97 if (i < BUFF_SIZE)
Maximousse 0:727643d2c6ef 98 buffer[i++] = pc.getc();
Maximousse 0:727643d2c6ef 99 }
Maximousse 0:727643d2c6ef 100 while (i > 0) {
Maximousse 0:727643d2c6ef 101 Command(buffer[i]);
Maximousse 0:727643d2c6ef 102 pi.cls();
Maximousse 0:727643d2c6ef 103 pi.locate(0,0);
Maximousse 0:727643d2c6ef 104 pi.print(&buffer[i], 1);
Maximousse 0:727643d2c6ef 105 i--;
Maximousse 0:727643d2c6ef 106 }
Maximousse 0:727643d2c6ef 107
Maximousse 1:236ac79b07d7 108 /*
Maximousse 0:727643d2c6ef 109 if(flag) {
Maximousse 0:727643d2c6ef 110 clavier+=1;
Maximousse 0:727643d2c6ef 111 pi.print(&clavier,1);
Maximousse 0:727643d2c6ef 112 //clavier = pc.getc();
Maximousse 0:727643d2c6ef 113
Maximousse 0:727643d2c6ef 114 switch (clavier) {
Maximousse 0:727643d2c6ef 115 case 'z':
Maximousse 0:727643d2c6ef 116 pi.forward(0.5);
Maximousse 0:727643d2c6ef 117 wait(0.5);
Maximousse 0:727643d2c6ef 118 pi.stop();
Maximousse 0:727643d2c6ef 119 break;
Maximousse 0:727643d2c6ef 120 case 'q':
Maximousse 0:727643d2c6ef 121 pi.left(0.5);
Maximousse 0:727643d2c6ef 122 wait(0.5);
Maximousse 0:727643d2c6ef 123 pi.stop();
Maximousse 0:727643d2c6ef 124 break;
Maximousse 0:727643d2c6ef 125 case 's':
Maximousse 0:727643d2c6ef 126 pi.backward(0.5);
Maximousse 0:727643d2c6ef 127 wait(0.5);
Maximousse 0:727643d2c6ef 128 pi.stop();
Maximousse 0:727643d2c6ef 129 break;
Maximousse 0:727643d2c6ef 130 case 'd':
Maximousse 0:727643d2c6ef 131 pi.right(0.5);
Maximousse 0:727643d2c6ef 132 wait(0.5);
Maximousse 0:727643d2c6ef 133 pi.stop();
Maximousse 0:727643d2c6ef 134 break;
Maximousse 0:727643d2c6ef 135 case 'b':
Maximousse 0:727643d2c6ef 136 readBattery();
Maximousse 0:727643d2c6ef 137 break;
Maximousse 0:727643d2c6ef 138 case 'c':
Maximousse 0:727643d2c6ef 139 readSensors();
Maximousse 0:727643d2c6ef 140 break;
Maximousse 0:727643d2c6ef 141 }
Maximousse 0:727643d2c6ef 142 flag=0;
Maximousse 0:727643d2c6ef 143 }
Maximousse 0:727643d2c6ef 144 */
Maximousse 0:727643d2c6ef 145 }
Maximousse 0:727643d2c6ef 146 }
Maximousse 0:727643d2c6ef 147