Polybot / Mbed 2 deprecated Polulu_TP_II_ET5

tp1

Dependencies:   mbed

main.cpp@0:727643d2c6ef, 2020-10-09 (annotated)

Committer:
Maximousse
Date:
Fri Oct 09 10:25:39 2020 +0000
Revision:
0:727643d2c6ef
Child:
1:236ac79b07d7
tp1

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