Tarek Lule / Bot_Auxiliaries

Auxiliaries I use for CreaBot

Bot_Auxiliaries.cpp@0:32b17da1ddae, 2018-10-31 (annotated)

Committer:
sepp_nepp
Date:
Wed Oct 31 14:22:26 2018 +0000
Revision:
0:32b17da1ddae
First Commit of Creabot Auxiliary functions

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sepp_nepp 0:32b17da1ddae 1 // *****************************************************************
sepp_nepp 0:32b17da1ddae 2 // Handle all the LEDs with some general function wrappers functions
sepp_nepp 0:32b17da1ddae 3 // *****************************************************************
sepp_nepp 0:32b17da1ddae 4 #include "Bot_Auxiliaries.h"
sepp_nepp 0:32b17da1ddae 5
sepp_nepp 0:32b17da1ddae 6 // Added a lookup function from a color number to a color, used by Creabot
sepp_nepp 0:32b17da1ddae 7 int LED_CAR::ColNr2Color( char ColNr)
sepp_nepp 0:32b17da1ddae 8 { switch (ColNr) {
sepp_nepp 0:32b17da1ddae 9 case 0: return BLACK;
sepp_nepp 0:32b17da1ddae 10 case 1: return WHITE;
sepp_nepp 0:32b17da1ddae 11 case 2: return RED;
sepp_nepp 0:32b17da1ddae 12 case 3: return GREEN;
sepp_nepp 0:32b17da1ddae 13 case 4: return BLUE;
sepp_nepp 0:32b17da1ddae 14 case 5: return PURPLE;
sepp_nepp 0:32b17da1ddae 15 default: return BLACK;
sepp_nepp 0:32b17da1ddae 16 }
sepp_nepp 0:32b17da1ddae 17 }
sepp_nepp 0:32b17da1ddae 18
sepp_nepp 0:32b17da1ddae 19 void LED_CAR::LEDsOff( void ) {
sepp_nepp 0:32b17da1ddae 20 StopRotation();
sepp_nepp 0:32b17da1ddae 21 StopBlink() ;
sepp_nepp 0:32b17da1ddae 22 for (int lNr=0; lNr<NumLEDs; lNr++)
sepp_nepp 0:32b17da1ddae 23 { SetColor(BLACK,lNr); }
sepp_nepp 0:32b17da1ddae 24 }
sepp_nepp 0:32b17da1ddae 25
sepp_nepp 0:32b17da1ddae 26 void LED_CAR::LEDsRainbow( void )
sepp_nepp 0:32b17da1ddae 27 { for (int Nr=0; Nr<NumLEDs; Nr++)
sepp_nepp 0:32b17da1ddae 28 { SetColor(ColNr2Color(Nr + 1),Nr); }
sepp_nepp 0:32b17da1ddae 29 }
sepp_nepp 0:32b17da1ddae 30
sepp_nepp 0:32b17da1ddae 31 void LED_CAR::LEDNrCol(LED_Nr aNr, int parameter)
sepp_nepp 0:32b17da1ddae 32 {
sepp_nepp 0:32b17da1ddae 33 SetColor( ColNr2Color( aNr ), aNr);
sepp_nepp 0:32b17da1ddae 34 }
sepp_nepp 0:32b17da1ddae 35
sepp_nepp 0:32b17da1ddae 36 void LED_CAR::LEDsRainbowMove( double speed )
sepp_nepp 0:32b17da1ddae 37 { LEDsRainbow( );
sepp_nepp 0:32b17da1ddae 38 StartRotation(0.3);
sepp_nepp 0:32b17da1ddae 39 }
sepp_nepp 0:32b17da1ddae 40
sepp_nepp 0:32b17da1ddae 41 void LED_CAR::LEDClignote(LED_Nr aNr, int OnOff) {
sepp_nepp 0:32b17da1ddae 42 if (OnOff == 1) {
sepp_nepp 0:32b17da1ddae 43 SetColor( ORANGE, aNr);
sepp_nepp 0:32b17da1ddae 44 StartBlink(0.5) ;}
sepp_nepp 0:32b17da1ddae 45 else { LEDsOff(); }
sepp_nepp 0:32b17da1ddae 46 }
sepp_nepp 0:32b17da1ddae 47
sepp_nepp 0:32b17da1ddae 48 // *****************************************************************
sepp_nepp 0:32b17da1ddae 49 // Handle all the LEDs specifically with the LED Position definitions
sepp_nepp 0:32b17da1ddae 50 // *****************************************************************
sepp_nepp 0:32b17da1ddae 51
sepp_nepp 0:32b17da1ddae 52 void LED_CAR::LEDFront(int ColNr)
sepp_nepp 0:32b17da1ddae 53 {
sepp_nepp 0:32b17da1ddae 54 SetColor( ColNr2Color( ColNr ), ledAvD);
sepp_nepp 0:32b17da1ddae 55 SetColor( ColNr2Color( ColNr ), ledAvG);
sepp_nepp 0:32b17da1ddae 56 }
sepp_nepp 0:32b17da1ddae 57
sepp_nepp 0:32b17da1ddae 58 void LED_CAR::LEDRear(int ColNr)
sepp_nepp 0:32b17da1ddae 59 {
sepp_nepp 0:32b17da1ddae 60 SetColor( ColNr2Color( ColNr ), ledArD);
sepp_nepp 0:32b17da1ddae 61 SetColor( ColNr2Color( ColNr ), ledArG);
sepp_nepp 0:32b17da1ddae 62 }
sepp_nepp 0:32b17da1ddae 63
sepp_nepp 0:32b17da1ddae 64 void LED_CAR::LEDCligR(int speed) {
sepp_nepp 0:32b17da1ddae 65 if ( (speed>0) && (speed<4) ) {
sepp_nepp 0:32b17da1ddae 66 SetColor(ORANGE, ledAvD) ;
sepp_nepp 0:32b17da1ddae 67 SetColor(ORANGE, ledArD) ;
sepp_nepp 0:32b17da1ddae 68 StartBlink(float(speed)/4) ;}
sepp_nepp 0:32b17da1ddae 69 else { LEDsOff(); }
sepp_nepp 0:32b17da1ddae 70 }
sepp_nepp 0:32b17da1ddae 71
sepp_nepp 0:32b17da1ddae 72 void LED_CAR::LEDCligL(int speed) {
sepp_nepp 0:32b17da1ddae 73 if ( (speed>0) && (speed<4) ) {
sepp_nepp 0:32b17da1ddae 74 SetColor(ORANGE, ledAvG) ;
sepp_nepp 0:32b17da1ddae 75 SetColor(ORANGE, ledArG) ;
sepp_nepp 0:32b17da1ddae 76 StartBlink(float(speed)/4) ;}
sepp_nepp 0:32b17da1ddae 77 else { LEDsOff(); }
sepp_nepp 0:32b17da1ddae 78 }
sepp_nepp 0:32b17da1ddae 79
sepp_nepp 0:32b17da1ddae 80 void LED_CAR::LEDAnim(int speed) {
sepp_nepp 0:32b17da1ddae 81 if ( (speed>0) && (speed<4) )
sepp_nepp 0:32b17da1ddae 82 { LEDsRainbowMove(float(speed)/4); }
sepp_nepp 0:32b17da1ddae 83 else { LEDsOff(); }
sepp_nepp 0:32b17da1ddae 84 }
sepp_nepp 0:32b17da1ddae 85
sepp_nepp 0:32b17da1ddae 86
sepp_nepp 0:32b17da1ddae 87 //************************************
sepp_nepp 0:32b17da1ddae 88 // Interpreter Class Creation
sepp_nepp 0:32b17da1ddae 89 //************************************
sepp_nepp 0:32b17da1ddae 90 // Strategy: the interpreter accumulates characters in its input buffer
sepp_nepp 0:32b17da1ddae 91 // it flags the presence of complete lines
sepp_nepp 0:32b17da1ddae 92 // it also flags Overflow of the buffer in which case all subsequent characters
sepp_nepp 0:32b17da1ddae 93 // are lost until the queue is emptied, and a CR-LF is received
sepp_nepp 0:32b17da1ddae 94 // Defined States of the interpreter:
sepp_nepp 0:32b17da1ddae 95 // MyState = isStartNew, isLineFilling, isOverflow, isWaitNewLine};
sepp_nepp 0:32b17da1ddae 96
sepp_nepp 0:32b17da1ddae 97
sepp_nepp 0:32b17da1ddae 98 Interpreter::Interpreter()
sepp_nepp 0:32b17da1ddae 99 { Reinit( );
sepp_nepp 0:32b17da1ddae 100 MyState = isStartNew;
sepp_nepp 0:32b17da1ddae 101 }
sepp_nepp 0:32b17da1ddae 102
sepp_nepp 0:32b17da1ddae 103 void Interpreter::Reinit( void ) {
sepp_nepp 0:32b17da1ddae 104 MyState = isWaitNewLine; // indicates no buffer overflow
sepp_nepp 0:32b17da1ddae 105 WriteIndex = 0; // points to next index to write to
sepp_nepp 0:32b17da1ddae 106 LinesComplete = 0; // indicates that no complete line is currently available
sepp_nepp 0:32b17da1ddae 107 // Start Scanning at the start
sepp_nepp 0:32b17da1ddae 108 ScanIndex = 0;
sepp_nepp 0:32b17da1ddae 109 BufAvail = BuffLen;// the full buffer size is available
sepp_nepp 0:32b17da1ddae 110 }
sepp_nepp 0:32b17da1ddae 111
sepp_nepp 0:32b17da1ddae 112 // Barebone function, assumes that checks havee been performed by writeBuf!
sepp_nepp 0:32b17da1ddae 113 void Interpreter::AddChar( char aChar ) {
sepp_nepp 0:32b17da1ddae 114 if (WriteIndex == BuffLen) {WriteIndex=0;}
sepp_nepp 0:32b17da1ddae 115 else {WriteIndex++;}
sepp_nepp 0:32b17da1ddae 116 RingBuf[WriteIndex]=aChar; // all right, buffer it!
sepp_nepp 0:32b17da1ddae 117 BufAvail--; // Buffer is shrinking
sepp_nepp 0:32b17da1ddae 118 if (BufAvail==0) { MyState = isOverflow; }
sepp_nepp 0:32b17da1ddae 119 }
sepp_nepp 0:32b17da1ddae 120
sepp_nepp 0:32b17da1ddae 121 // High level method to add a Char to the buffer,
sepp_nepp 0:32b17da1ddae 122 // Separates at Chars 10, 13, 0; replaced by a single 0 char
sepp_nepp 0:32b17da1ddae 123 // Blocking write when buffer has overflowed
sepp_nepp 0:32b17da1ddae 124 void Interpreter::writeBuf(char aChar) {
sepp_nepp 0:32b17da1ddae 125 bool LineEnds = aChar==10 || aChar==13 || aChar==0;
sepp_nepp 0:32b17da1ddae 126 switch (MyState) {
sepp_nepp 0:32b17da1ddae 127 case isOverflow: break;
sepp_nepp 0:32b17da1ddae 128 case isStartNew:
sepp_nepp 0:32b17da1ddae 129 // avoid that consecutive CR LF are counted as multiple lines
sepp_nepp 0:32b17da1ddae 130 if (!LineEnds) {AddChar(aChar ); MyState = isLineFilling; }
sepp_nepp 0:32b17da1ddae 131 break;
sepp_nepp 0:32b17da1ddae 132 case isLineFilling:
sepp_nepp 0:32b17da1ddae 133 if (!LineEnds) {AddChar(aChar);}
sepp_nepp 0:32b17da1ddae 134 else
sepp_nepp 0:32b17da1ddae 135 {MyState = isStartNew; // ready for the next line
sepp_nepp 0:32b17da1ddae 136 // Between consecutive commands, the endstring=NULL character is inserted
sepp_nepp 0:32b17da1ddae 137 // this is to indicate that line was already counted as completed
sepp_nepp 0:32b17da1ddae 138 AddChar(0 ); // append a line end char, will detect bufferFull!
sepp_nepp 0:32b17da1ddae 139 LinesComplete++; } // count completed lines
sepp_nepp 0:32b17da1ddae 140 break;
sepp_nepp 0:32b17da1ddae 141 case isWaitNewLine: // waiting for a new line end to arrive after an overflow
sepp_nepp 0:32b17da1ddae 142 if (LineEnds) { MyState = isStartNew; }
sepp_nepp 0:32b17da1ddae 143 break;
sepp_nepp 0:32b17da1ddae 144 default: MyState = isOverflow; // goes into error state, should never happen though
sepp_nepp 0:32b17da1ddae 145 }
sepp_nepp 0:32b17da1ddae 146 } // writeBuf
sepp_nepp 0:32b17da1ddae 147
sepp_nepp 0:32b17da1ddae 148 // Barebone function, that performs the checks to be performed!
sepp_nepp 0:32b17da1ddae 149 // Passes back the actC, and reads already the nextChar into actC
sepp_nepp 0:32b17da1ddae 150 char Interpreter::GetAChar( void ) {
sepp_nepp 0:32b17da1ddae 151 char oldC = actC;
sepp_nepp 0:32b17da1ddae 152 if (BufAvail==BuffLen) { actC = 0; } // buffer is empty
sepp_nepp 0:32b17da1ddae 153 else // something is in the buffer
sepp_nepp 0:32b17da1ddae 154 { if (ScanIndex == BuffLen) {ScanIndex=0;}
sepp_nepp 0:32b17da1ddae 155 else {ScanIndex++;}
sepp_nepp 0:32b17da1ddae 156 actC=RingBuf[ScanIndex]; // all right, get it
sepp_nepp 0:32b17da1ddae 157 BufAvail++; // Buffer is increasing
sepp_nepp 0:32b17da1ddae 158 } // something is in the buffer
sepp_nepp 0:32b17da1ddae 159 return oldC;
sepp_nepp 0:32b17da1ddae 160 }
sepp_nepp 0:32b17da1ddae 161
sepp_nepp 0:32b17da1ddae 162 // skip true blank, but also Tab characters
sepp_nepp 0:32b17da1ddae 163 void Interpreter::SkipBlanks( void ) {
sepp_nepp 0:32b17da1ddae 164 while(BufAvail<BuffLen && (actC==' ' || actC==9|| actC==0)) { GetAChar(); }
sepp_nepp 0:32b17da1ddae 165 } // SkipBlanks
sepp_nepp 0:32b17da1ddae 166
sepp_nepp 0:32b17da1ddae 167 int Interpreter::ReadAnInt( void ) {
sepp_nepp 0:32b17da1ddae 168 bool Negative = false;
sepp_nepp 0:32b17da1ddae 169 int Result = 0;
sepp_nepp 0:32b17da1ddae 170 if (actC=='-') {Negative = true; GetAChar(); }
sepp_nepp 0:32b17da1ddae 171 else if (actC=='+') {Negative = false; GetAChar(); }
sepp_nepp 0:32b17da1ddae 172 while(BufAvail<BuffLen && actC>='0' && actC<='9')
sepp_nepp 0:32b17da1ddae 173 { Result = Result * 10 + (GetAChar()-'0') ; }
sepp_nepp 0:32b17da1ddae 174 return Negative? -Result : Result;
sepp_nepp 0:32b17da1ddae 175 } // ReadAnInt
sepp_nepp 0:32b17da1ddae 176
sepp_nepp 0:32b17da1ddae 177
sepp_nepp 0:32b17da1ddae 178 RD_CMD_TYPE Interpreter::ParseCommand( void ) {
sepp_nepp 0:32b17da1ddae 179 RD_CMD_TYPE cmd; // locally built command
sepp_nepp 0:32b17da1ddae 180 actC=RingBuf[ScanIndex];
sepp_nepp 0:32b17da1ddae 181 SkipBlanks();
sepp_nepp 0:32b17da1ddae 182 // Next Character is the command
sepp_nepp 0:32b17da1ddae 183 cmd.Command = GetAChar();
sepp_nepp 0:32b17da1ddae 184 // Next Blanks are to be omitted, but are not even mandatory
sepp_nepp 0:32b17da1ddae 185 SkipBlanks();
sepp_nepp 0:32b17da1ddae 186 if ((actC>='0' && actC<='9') || actC=='-' || actC=='+' )
sepp_nepp 0:32b17da1ddae 187 { cmd.Parameter= ReadAnInt(); cmd.NumParam = 1; }
sepp_nepp 0:32b17da1ddae 188 else { cmd.Parameter= 0; cmd.NumParam = 0; }
sepp_nepp 0:32b17da1ddae 189 SkipBlanks( ); // There should be at least a trailing NUL character to be removed
sepp_nepp 0:32b17da1ddae 190 LinesComplete--; // one line less in the storage
sepp_nepp 0:32b17da1ddae 191
sepp_nepp 0:32b17da1ddae 192 return cmd; // return the built command
sepp_nepp 0:32b17da1ddae 193 }
sepp_nepp 0:32b17da1ddae 194
sepp_nepp 0:32b17da1ddae 195
sepp_nepp 0:32b17da1ddae 196
sepp_nepp 0:32b17da1ddae 197