Adrian Nowak / Mbed 2 deprecated __M2__5b____copy

ll

Dependencies:   mbed

main.cpp@0:a8e3f4b25a52, 2017-05-09 (annotated)

Committer:
adrianow795
Date:
Tue May 09 11:59:23 2017 +0000
Revision:
0:a8e3f4b25a52
siema;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
adrianow795 0:a8e3f4b25a52 1 #include "mbed.h"
adrianow795 0:a8e3f4b25a52 2 #include "string.h"
adrianow795 0:a8e3f4b25a52 3 #include "command_decoder.h"
adrianow795 0:a8e3f4b25a52 4 #include "servo.h"
adrianow795 0:a8e3f4b25a52 5
adrianow795 0:a8e3f4b25a52 6 DigitalOut led(LED1);
adrianow795 0:a8e3f4b25a52 7 DigitalOut led2(LED2);
adrianow795 0:a8e3f4b25a52 8
adrianow795 0:a8e3f4b25a52 9 Serial pc(USBTX, USBRX);
adrianow795 0:a8e3f4b25a52 10
adrianow795 0:a8e3f4b25a52 11 //================================================
adrianow795 0:a8e3f4b25a52 12 // PUT_STRING
adrianow795 0:a8e3f4b25a52 13 //================================================
adrianow795 0:a8e3f4b25a52 14
adrianow795 0:a8e3f4b25a52 15 int puts( char * tab , int size )
adrianow795 0:a8e3f4b25a52 16 {
adrianow795 0:a8e3f4b25a52 17 int ret_val;
adrianow795 0:a8e3f4b25a52 18 for ( int i = 0; i < size; i++)
adrianow795 0:a8e3f4b25a52 19 {
adrianow795 0:a8e3f4b25a52 20 if(tab[i] == NULL)
adrianow795 0:a8e3f4b25a52 21 {
adrianow795 0:a8e3f4b25a52 22 ret_val = 0;
adrianow795 0:a8e3f4b25a52 23 tab[i] = '\r';
adrianow795 0:a8e3f4b25a52 24 break;
adrianow795 0:a8e3f4b25a52 25 }
adrianow795 0:a8e3f4b25a52 26 else
adrianow795 0:a8e3f4b25a52 27 {
adrianow795 0:a8e3f4b25a52 28 ret_val = 1;
adrianow795 0:a8e3f4b25a52 29 }
adrianow795 0:a8e3f4b25a52 30 }
adrianow795 0:a8e3f4b25a52 31
adrianow795 0:a8e3f4b25a52 32 if(ret_val == 0)
adrianow795 0:a8e3f4b25a52 33 {
adrianow795 0:a8e3f4b25a52 34 while(*tab != '\r')
adrianow795 0:a8e3f4b25a52 35 {
adrianow795 0:a8e3f4b25a52 36 if(pc.writeable())
adrianow795 0:a8e3f4b25a52 37 {
adrianow795 0:a8e3f4b25a52 38 pc.putc((int)(*tab));
adrianow795 0:a8e3f4b25a52 39 tab++;
adrianow795 0:a8e3f4b25a52 40 }
adrianow795 0:a8e3f4b25a52 41 }
adrianow795 0:a8e3f4b25a52 42 pc.putc((int)(*tab));
adrianow795 0:a8e3f4b25a52 43 }
adrianow795 0:a8e3f4b25a52 44 else
adrianow795 0:a8e3f4b25a52 45 {
adrianow795 0:a8e3f4b25a52 46 //empty else
adrianow795 0:a8e3f4b25a52 47 }
adrianow795 0:a8e3f4b25a52 48
adrianow795 0:a8e3f4b25a52 49
adrianow795 0:a8e3f4b25a52 50 return ret_val;
adrianow795 0:a8e3f4b25a52 51 }
adrianow795 0:a8e3f4b25a52 52
adrianow795 0:a8e3f4b25a52 53 //================================================
adrianow795 0:a8e3f4b25a52 54 // GET_STRING
adrianow795 0:a8e3f4b25a52 55 //================================================
adrianow795 0:a8e3f4b25a52 56
adrianow795 0:a8e3f4b25a52 57 int gets( char * tab , int size )
adrianow795 0:a8e3f4b25a52 58 {
adrianow795 0:a8e3f4b25a52 59 int Counter = 0, ret_val = 0;
adrianow795 0:a8e3f4b25a52 60 while((*tab = pc.getc()) != '\r')
adrianow795 0:a8e3f4b25a52 61 {
adrianow795 0:a8e3f4b25a52 62 if(Counter < size)
adrianow795 0:a8e3f4b25a52 63 {
adrianow795 0:a8e3f4b25a52 64
adrianow795 0:a8e3f4b25a52 65 }
adrianow795 0:a8e3f4b25a52 66 else
adrianow795 0:a8e3f4b25a52 67 {
adrianow795 0:a8e3f4b25a52 68 ret_val = 1;
adrianow795 0:a8e3f4b25a52 69 }
adrianow795 0:a8e3f4b25a52 70 Counter++;
adrianow795 0:a8e3f4b25a52 71 tab++;
adrianow795 0:a8e3f4b25a52 72 }
adrianow795 0:a8e3f4b25a52 73
adrianow795 0:a8e3f4b25a52 74 *tab = NULL;
adrianow795 0:a8e3f4b25a52 75
adrianow795 0:a8e3f4b25a52 76 if(ret_val)
adrianow795 0:a8e3f4b25a52 77 while(pc.readable())
adrianow795 0:a8e3f4b25a52 78 {
adrianow795 0:a8e3f4b25a52 79 pc.getc();
adrianow795 0:a8e3f4b25a52 80 }
adrianow795 0:a8e3f4b25a52 81
adrianow795 0:a8e3f4b25a52 82 return ret_val;
adrianow795 0:a8e3f4b25a52 83 }
adrianow795 0:a8e3f4b25a52 84
adrianow795 0:a8e3f4b25a52 85 //================================================
adrianow795 0:a8e3f4b25a52 86 // INT_TO_CHAR
adrianow795 0:a8e3f4b25a52 87 //================================================
adrianow795 0:a8e3f4b25a52 88
adrianow795 0:a8e3f4b25a52 89 void IntToChar ( int number, char * wynik)
adrianow795 0:a8e3f4b25a52 90 {
adrianow795 0:a8e3f4b25a52 91 // zakres do tysiecy
adrianow795 0:a8e3f4b25a52 92 int temp = 0;
adrianow795 0:a8e3f4b25a52 93
adrianow795 0:a8e3f4b25a52 94 for (int i = 3 ; i >= 0; i--)
adrianow795 0:a8e3f4b25a52 95 {
adrianow795 0:a8e3f4b25a52 96 temp = number % 10;
adrianow795 0:a8e3f4b25a52 97 wynik [i] = 48 + temp;
adrianow795 0:a8e3f4b25a52 98 number = number - temp;
adrianow795 0:a8e3f4b25a52 99 number = number / 10;
adrianow795 0:a8e3f4b25a52 100 }
adrianow795 0:a8e3f4b25a52 101 }
adrianow795 0:a8e3f4b25a52 102
adrianow795 0:a8e3f4b25a52 103 //================================================
adrianow795 0:a8e3f4b25a52 104 // COMPARE_STRINGS
adrianow795 0:a8e3f4b25a52 105 //================================================
adrianow795 0:a8e3f4b25a52 106
adrianow795 0:a8e3f4b25a52 107 int compstr( char * a, char * b)
adrianow795 0:a8e3f4b25a52 108 {
adrianow795 0:a8e3f4b25a52 109
adrianow795 0:a8e3f4b25a52 110 while(*b != NULL )
adrianow795 0:a8e3f4b25a52 111 if(*a == *b)
adrianow795 0:a8e3f4b25a52 112 {
adrianow795 0:a8e3f4b25a52 113 a++;
adrianow795 0:a8e3f4b25a52 114 b++;
adrianow795 0:a8e3f4b25a52 115 }
adrianow795 0:a8e3f4b25a52 116 else
adrianow795 0:a8e3f4b25a52 117 return 0;
adrianow795 0:a8e3f4b25a52 118 return 1;
adrianow795 0:a8e3f4b25a52 119 }
adrianow795 0:a8e3f4b25a52 120
adrianow795 0:a8e3f4b25a52 121 //================================================
adrianow795 0:a8e3f4b25a52 122 // ToLower
adrianow795 0:a8e3f4b25a52 123 //================================================
adrianow795 0:a8e3f4b25a52 124
adrianow795 0:a8e3f4b25a52 125 void ToLower(char * chain, int size)
adrianow795 0:a8e3f4b25a52 126 {
adrianow795 0:a8e3f4b25a52 127 for(int i = 0; i < size; i++)
adrianow795 0:a8e3f4b25a52 128 {
adrianow795 0:a8e3f4b25a52 129 if(chain[i] >= 65 && chain[i] <= 90)
adrianow795 0:a8e3f4b25a52 130 {
adrianow795 0:a8e3f4b25a52 131 chain[i] = chain[i] + 32;
adrianow795 0:a8e3f4b25a52 132 }
adrianow795 0:a8e3f4b25a52 133 else
adrianow795 0:a8e3f4b25a52 134 {
adrianow795 0:a8e3f4b25a52 135 //empty else
adrianow795 0:a8e3f4b25a52 136 }
adrianow795 0:a8e3f4b25a52 137 }
adrianow795 0:a8e3f4b25a52 138 }
adrianow795 0:a8e3f4b25a52 139
adrianow795 0:a8e3f4b25a52 140 //================================================
adrianow795 0:a8e3f4b25a52 141 // ClearTab
adrianow795 0:a8e3f4b25a52 142 //================================================
adrianow795 0:a8e3f4b25a52 143
adrianow795 0:a8e3f4b25a52 144 void ClearTab(char * tab, int size)
adrianow795 0:a8e3f4b25a52 145 {
adrianow795 0:a8e3f4b25a52 146 for(int i = 0; i < size; i++)
adrianow795 0:a8e3f4b25a52 147 tab[i] = NULL;
adrianow795 0:a8e3f4b25a52 148 }
adrianow795 0:a8e3f4b25a52 149
adrianow795 0:a8e3f4b25a52 150 void FillTab(char * tab, const char * data, int size)
adrianow795 0:a8e3f4b25a52 151 {
adrianow795 0:a8e3f4b25a52 152 ClearTab(tab, size);
adrianow795 0:a8e3f4b25a52 153 while(*data != NULL)
adrianow795 0:a8e3f4b25a52 154 {
adrianow795 0:a8e3f4b25a52 155 *tab = *data;
adrianow795 0:a8e3f4b25a52 156 data++;
adrianow795 0:a8e3f4b25a52 157 tab++;
adrianow795 0:a8e3f4b25a52 158 }
adrianow795 0:a8e3f4b25a52 159 *tab = *data;
adrianow795 0:a8e3f4b25a52 160
adrianow795 0:a8e3f4b25a52 161 }
adrianow795 0:a8e3f4b25a52 162
adrianow795 0:a8e3f4b25a52 163 extern unsigned char ucTokenNr;
adrianow795 0:a8e3f4b25a52 164 extern struct Token asToken[MAX_TOKEN_NR];
adrianow795 0:a8e3f4b25a52 165
adrianow795 0:a8e3f4b25a52 166
adrianow795 0:a8e3f4b25a52 167 int main()
adrianow795 0:a8e3f4b25a52 168 {
adrianow795 0:a8e3f4b25a52 169
adrianow795 0:a8e3f4b25a52 170 char Receive [24];
adrianow795 0:a8e3f4b25a52 171 Servo_Init(10);
adrianow795 0:a8e3f4b25a52 172 while(1) {
adrianow795 0:a8e3f4b25a52 173
adrianow795 0:a8e3f4b25a52 174 if( gets(Receive,24) == 0 )
adrianow795 0:a8e3f4b25a52 175 {
adrianow795 0:a8e3f4b25a52 176 DecodeMsg(Receive);
adrianow795 0:a8e3f4b25a52 177
adrianow795 0:a8e3f4b25a52 178 {
adrianow795 0:a8e3f4b25a52 179 switch(asToken[0].uValue.eKeyword){
adrianow795 0:a8e3f4b25a52 180 case ID:
adrianow795 0:a8e3f4b25a52 181 FillTab(Receive, "id 3234", 24);
adrianow795 0:a8e3f4b25a52 182 puts(Receive,24);
adrianow795 0:a8e3f4b25a52 183 break;
adrianow795 0:a8e3f4b25a52 184 case CALLIB:
adrianow795 0:a8e3f4b25a52 185 Servo_Callib();
adrianow795 0:a8e3f4b25a52 186 FillTab(Receive, "callib_ok", 24);
adrianow795 0:a8e3f4b25a52 187 puts(Receive,24);
adrianow795 0:a8e3f4b25a52 188 led = !led;
adrianow795 0:a8e3f4b25a52 189 break;
adrianow795 0:a8e3f4b25a52 190 case GOTO:
adrianow795 0:a8e3f4b25a52 191 Servo_GoTo(asToken[1].uValue.uiNumber);
adrianow795 0:a8e3f4b25a52 192 FillTab(Receive, "goto_ok", 24);
adrianow795 0:a8e3f4b25a52 193 puts(Receive,24);
adrianow795 0:a8e3f4b25a52 194 led = !led;
adrianow795 0:a8e3f4b25a52 195 break;
adrianow795 0:a8e3f4b25a52 196 case STEP:
adrianow795 0:a8e3f4b25a52 197 Servo_Step(asToken[1].uValue.uiNumber);
adrianow795 0:a8e3f4b25a52 198 FillTab(Receive, "step_ok", 24);
adrianow795 0:a8e3f4b25a52 199 puts(Receive,24);
adrianow795 0:a8e3f4b25a52 200 led = !led;
adrianow795 0:a8e3f4b25a52 201 break;
adrianow795 0:a8e3f4b25a52 202 default:
adrianow795 0:a8e3f4b25a52 203 FillTab(Receive, "unknowncommend", 24);
adrianow795 0:a8e3f4b25a52 204 puts(Receive,24);
adrianow795 0:a8e3f4b25a52 205 break;
adrianow795 0:a8e3f4b25a52 206 }
adrianow795 0:a8e3f4b25a52 207 }
adrianow795 0:a8e3f4b25a52 208 }
adrianow795 0:a8e3f4b25a52 209 }
adrianow795 0:a8e3f4b25a52 210 }
adrianow795 0:a8e3f4b25a52 211
adrianow795 0:a8e3f4b25a52 212