Arduino Source Code Update

Split the code into tabs to have an easier access:

  1. Destroyer_3000.ino for global functions and variables;
  2. FSM.ino for Finite State Machine functions;
  3. Loop.ino for critical actions and functions;
  4. Motor.ino for motor speed control;
  5. PWM.ino just for prescaler choice;
  6. Serial_Monitor.ino for sensor data monitoring;
  7. Setup.ino for all the data setup and single run functions;
  8. definitions.h for all global constants.
#include <FiniteStateMachine.h>
Added some more states:
State idleRunState = State(idleRunEnter, idleRunUpdate, NULL); State spinState = State(spinEnter, spinUpdate, spinExit); State lineAvoidState = State(lineAvoidEnter, lineAvoidUpdate, NULL); State attackState = State(attackEnter, attackUpdate, NULL); State overfallAvoidState = State(overfallAvoidEnter, overfallAvoidUpdate, NULL); FiniteStateMachine stateMachine = FiniteStateMachine(idleRunState);
The data sum of 8 sensors might have the result from 0~255 as a byte, so it is not efficient to write a switch-case for that. In order to find all combinations, use windows calc.exe in programmer mode for 1 byte in decimal units. Click "+", "1" and keep clicking "=" to increment the value, check the binary code while clicking.

"B00010001" states that proximity sensor on the left side and left line sensor are active at the same time, but priority is to avoid the line of the Dohyo, so the number "17" will be included in the interval which activates the "lineAvoidState".

During finite state, just toggle constant forward speed for patrolling.
/*IDLE RUN STATE*/ void idleRunEnter(){   motor(50, 50); }
Update it with continuous sensor readings, several if's will do the transitions.
(Update 2012.12.14 17:57): Fixed the condition for attackState, was the same as spinState's, in result robot runs stright forward as forever in idleEnter function, no line detection.
void idleRunUpdate(){   SensorSum();   if(sensorSum==1||sensorSum==3||sensorSum==8||sensorSum==12){     stateMachine.transitionTo(spinState);   }   if((sensorSum>=16 && sensorSum<=63)||(sensorSum>=208 && sensorSum<=255)){     stateMachine.transitionTo(lineAvoidState);   }   if(sensorSum==2||sensorSum==4||sensorSum==6||sensorSum==7||sensorSum==14){     stateMachine.transitionTo(attackState);   }   if((sensorSum>=64 && sensorSum<=79)||(sensorSum>=128 && sensorSum<=143)||(sensorSum>=192 && sensorSum<=207)){     stateMachine.transitionTo(overfallAvoidState);   } }
Under line avoid state are some setups on the enter: a timer, mask, a switch. 2 line sensors are used, so there is 3 combinations of them: left, right and both. Each result drives the system accordingly.
/*LINE AVOID STATE*/void lineAvoidEnter(){   timeOld=millis();   sensorMask=!(sensorSum & 48);   switch(sensorSum & 48){     case 16: timer=600; motor( 0, -50); break;     case 32: timer=600; motor(-50, 0); break;     case 48: timer=1000; motor(-50, -50); break;   } }
While avoiding the line, scan the sensor readings for a change and ignore the previous sensor, while in state in order not to tristate the state. The state will transit back if timer is out or there is a positive sensor result. But there is a condition for both activated line sensors, it will transit to spin state if no sensor was found after timeout.
void lineAvoidUpdate(){   SensorSum();   if(sensorSum & sensorMask > 0 || millis()-timeOld >=timer*8){     if(sensorMask==!48 && millis()-timeOld>=timer*8){       timeOld=millis();       stateMachine.immediateTransitionTo(spinState);     }     stateMachine.immediateTransitionTo(idleRunState);   } }
Analogical situation in the spin state.
/*SPIN STATE*/void spinEnter(){   timeOld=millis();   sensorMask=!(sensorSum & 9);   switch(sensorSum & 9){     case 0: timer=1000; motor(-50, +50); break;     case 1: timer=1000; motor(-50, +50); break;     case 8: timer=1000; motor(+50, -50); break;   } } void spinUpdate(){   SensorSum();   if(sensorSum & sensorMask > 0 || millis()-timeOld >=timer*8){     stateMachine.immediateTransitionTo(idleRunState);   } } void spinExit(){ }
Functions not programmed yet.
void attackEnter(){ } void attackUpdate(){ } void attackExit(){ } void overfallAvoidEnter(){ } void overfallAvoidUpdate(){ } void overfallAvoidExit(){ }

No comments:

Post a Comment