Commit 8167aac376d075ffcf4cae4d95e54bda50d6b07a

Authored by Mestari Yousra
1 parent 7543a6ee

Tout neuf. voir ligne 470

Showing 1 changed file with 1054 additions and 0 deletions   Show diff stats
... ... @@ -0,0 +1,1054 @@
  1 +/************************* (C) COPYRIGHT 2010 ROBOTIS **************************
  2 + * File Name : main.c
  3 + * Author : danceww
  4 + * Version : V0.0.1
  5 + * Date : 08/23/2010
  6 + * Description : Main program body
  7 + *******************************************************************************/
  8 +
  9 +/* Includes ------------------------------------------------------------------*/
  10 +
  11 +//#include "includes.h"
  12 +#include "stm32f10x_lib.h"
  13 +#include "dynamixel.h"
  14 +#include "dxl_hal.h"
  15 +
  16 +#include "AX12.h"
  17 +#include "AXS1.h"
  18 +
  19 +/* Private typedef -----------------------------------------------------------*/
  20 +/* Private define ------------------------------------------------------------*/
  21 +#define P_GOAL_POSITION_L 30
  22 +#define P_GOAL_POSITION_H 31
  23 +#define P_PRESENT_POSITION_L 36
  24 +#define P_PRESENT_POSITION_H 37
  25 +#define P_MOVING 46
  26 +
  27 +#define PORT_ENABLE_TXD GPIOB
  28 +#define PORT_ENABLE_RXD GPIOB
  29 +#define PORT_DXL_TXD GPIOB
  30 +#define PORT_DXL_RXD GPIOB
  31 +
  32 +
  33 +#define PIN_ENABLE_TXD GPIO_Pin_4
  34 +#define PIN_ENABLE_RXD GPIO_Pin_5
  35 +#define PIN_DXL_TXD GPIO_Pin_6
  36 +#define PIN_DXL_RXD GPIO_Pin_7
  37 +#define PIN_PC_TXD GPIO_Pin_10
  38 +#define PIN_PC_RXD GPIO_Pin_11
  39 +
  40 +
  41 +#define PORT_LED_AUX GPIOB
  42 +#define PORT_LED_MANAGE GPIOB
  43 +#define PORT_LED_PROGRAM GPIOB
  44 +#define PORT_LED_PLAY GPIOB
  45 +#define PORT_LED_POWER GPIOC
  46 +#define PORT_LED_TX GPIOC
  47 +#define PORT_LED_RX GPIOC
  48 +
  49 +#define PIN_LED_AUX GPIO_Pin_12
  50 +#define PIN_LED_MANAGE GPIO_Pin_13
  51 +#define PIN_LED_PROGRAM GPIO_Pin_14
  52 +#define PIN_LED_PLAY GPIO_Pin_15
  53 +#define PIN_LED_POWER GPIO_Pin_13
  54 +#define PIN_LED_TX GPIO_Pin_14
  55 +#define PIN_LED_RX GPIO_Pin_15
  56 +
  57 +
  58 +#define USART_DXL 0
  59 +#define USART_PC 2
  60 +
  61 +#define word u16
  62 +#define byte u8
  63 +
  64 +/* Private macro -------------------------------------------------------------*/
  65 +/* Private variables ---------------------------------------------------------*/
  66 +volatile byte gbpRxInterruptBuffer[256]; // dxl buffer
  67 +volatile byte gbRxBufferWritePointer,gbRxBufferReadPointer;
  68 +volatile vu32 gwTimingDelay,gw1msCounter;
  69 +u32 Baudrate_DXL = 1000000;
  70 +u32 Baudrate_PC = 57600;
  71 +vu16 CCR1_Val = 100; // 1ms
  72 +vu32 capture = 0;
  73 +word GoalPos[2] = {0, 1023};
  74 +//word GoalPos[2] = {0, 1023}; //For EX and MX series
  75 +word Position;
  76 +word wPresentPos;
  77 +byte INDEX = 0;
  78 +byte Voltage;
  79 +byte id = 1;
  80 +byte bMoving, CommStatus;
  81 +
  82 +/* Private function prototypes -----------------------------------------------*/
  83 +/* Private functions ---------------------------------------------------------*/
  84 +void RCC_Configuration(void);
  85 +void NVIC_Configuration(void);
  86 +void GPIO_Configuration(void);
  87 +void SysTick_Configuration(void);
  88 +void Timer_configuration(void);
  89 +void TimerInterrupt_1ms(void);
  90 +void RxD0Interrupt(void);
  91 +void __ISR_DELAY(void);
  92 +void USART1_Configuration(u32);
  93 +void USART_Configuration(u8, u32);
  94 +void DisableUSART1(void);
  95 +void ClearBuffer256(void);
  96 +byte CheckNewArrive(void);
  97 +void PrintCommStatus(int);
  98 +void PrintErrorCode(void);
  99 +void TxDByte_DXL(byte);
  100 +byte RxDByte_DXL(void);
  101 +void TxDString(byte*);
  102 +void TxDWord16(word);
  103 +void TxDByte16(byte);
  104 +void TxDByte_PC(byte);
  105 +void Timer_Configuration(void);
  106 +void mDelay(u32);
  107 +void StartDiscount(s32);
  108 +byte CheckTimeOut(void);
  109 +
  110 +
  111 +////////////////////////////////////////////////////////////
  112 +///////////////////// value definitions ////////////////////
  113 +////////////////////////////////////////////////////////////
  114 +
  115 +// change IDs here if the IDs of your motors/sensors are different
  116 +#define MOTOR_down_right 2
  117 +#define MOTOR_down_left 3
  118 +#define MOTOR_up_left 4
  119 +#define MOTOR_up_right 1
  120 +#define SENSOR 100
  121 +
  122 +
  123 +// state machine constants
  124 +#define INIT 0
  125 +#define GO_TO_CENTER 1
  126 +#define SEEKING 2
  127 +#define CHASING 3
  128 +#define STOP -1
  129 +
  130 +
  131 +#define speed_ini 400
  132 +#define speed_max 512
  133 +
  134 +
  135 +//////////////////////////////////////
  136 +/////////////// AX 12 ////////////////
  137 +//////////////////////////////////////
  138 +
  139 +// infinite turn mode activation, see technical docu
  140 +// parameter: ID of motor
  141 +void infiniteTurn(unsigned char id) {
  142 + dxl_write_word(id, AX12_CTAB_ID_CWAngleLimitLo, 0 ) ;
  143 + int result = dxl_get_result();
  144 + dxl_write_word(id, AX12_CTAB_ID_CCWAngleLimitLo, 0 ) ;
  145 + result = dxl_get_result();
  146 + TxDString("\nCM5 infinite rotation mode set\n");
  147 +}
  148 +
  149 +
  150 +// infinite turn mode desactivation, see technical docu
  151 +// parameter: ID of motor
  152 +void normalTurn(unsigned char id) {
  153 + dxl_write_word(id, AX12_CTAB_ID_CWAngleLimitLo, 0 ) ;
  154 + int result = dxl_get_result();
  155 + dxl_write_word(id, AX12_CTAB_ID_CCWAngleLimitLo, 1023 ) ;
  156 + result = dxl_get_result();
  157 + TxDString("\nCM5 normal rotation mode set\n");
  158 +}
  159 +
  160 +
  161 +// set rotation speed of a single motor, only works in infinite turn mode!
  162 +// speed is an integer between -1023 and 1023
  163 +// parameter motor: ID of motor
  164 +// parameter speed: rotation speed, between -1024 and 1024, sign controls direction
  165 +// speed 1 = no ratation, speed 0 = maximal speed
  166 +void setSpeed(unsigned char id, int speed) {
  167 + int order;
  168 + if(speed >= 0)
  169 + order = speed;
  170 + else
  171 + order = 1024 - speed;
  172 + dxl_write_word(id, AX12_CTAB_ID_MovingSpeedLo, order ) ;
  173 + int result = dxl_get_result();
  174 + if( result != COMM_RXSUCCESS )
  175 + {
  176 + TxDString("problem, code=");
  177 + TxDWord16(result);
  178 + TxDString("!!!\n");
  179 + }
  180 +}
  181 +
  182 +// move motor to a given angle, only works when nOt in infinite turn mode
  183 +// parameter motor: ID of motor
  184 +// parameter: angle is an integer between -1023 and 1023
  185 +// no angle should be between 300 and 360 degrees
  186 +void setAngle(unsigned char id, int angle, int speed) {
  187 + setSpeed(id, speed);
  188 + int angle_norm;
  189 +
  190 + if (angle >=0)
  191 + angle_norm = angle;
  192 +
  193 + else
  194 + angle_norm = 1024 + angle;
  195 +
  196 + dxl_write_word(id, AX12_CTAB_ID_GoalPositionLo, angle_norm ) ;
  197 + int result = dxl_get_result();
  198 + if( result != COMM_RXSUCCESS )
  199 + {
  200 + TxDString("\nproblem, code==");
  201 + TxDWord16(result);
  202 + TxDString("!!!\n");
  203 + }
  204 +}
  205 +
  206 +
  207 +// turns on motor light
  208 +// parameter motor: ID of motor
  209 +void lightOn(unsigned char id) {
  210 + dxl_write_byte(id, AX12_CTAB_ID_Led, 1 ) ;
  211 + int result = dxl_get_result();
  212 + if( result != COMM_RXSUCCESS )
  213 + {
  214 + TxDString("\nproblem, code==");
  215 + TxDWord16(result);
  216 + TxDString("!!!\n");
  217 + }
  218 +}
  219 +
  220 +// turns off motor light
  221 +// parameter motor: ID of motor
  222 +void lightOff(unsigned char id) {
  223 + dxl_write_byte(id, AX12_CTAB_ID_Led, 0 ) ;
  224 + int result = dxl_get_result();
  225 + if( result != COMM_RXSUCCESS )
  226 + {
  227 + TxDString("\nproblem, code==");
  228 + TxDWord16(result);
  229 + TxDString("!!!\n");
  230 + }
  231 +}
  232 +
  233 +// returns the current motor's speed
  234 +// This functions does not return anything but stores the speed in its 2nd parameter which lust be a pointer to int
  235 +// parameter inId: ID of motor
  236 +// parameter outSpeed: pointer to which the speed will be stored
  237 +void getSpeed(unsigned char id, unsigned int* outSpeed) {
  238 + *outSpeed = dxl_read_word(id, AX12_CTAB_ID_MovingSpeedLo) ;
  239 + int result = dxl_get_result();
  240 + if( result != COMM_RXSUCCESS )
  241 + {
  242 + TxDString("\nproblem, code=");
  243 + TxDWord16(result);
  244 + TxDString("!!!\n");
  245 + }
  246 +}
  247 +
  248 +
  249 +// returns the current motor's angle, infinite turn must be disabled to use this function
  250 +// This functions does not return anything but stores the speed in its 2nd parameter which lust be a pointer to int
  251 +// parameter inId: ID of motor
  252 +// parameter outSpeed: pointer to which the speed will be stored
  253 +void getAngle(unsigned char id, unsigned int* outAngle) {
  254 + *outAngle = dxl_read_word(id, AX12_CTAB_ID_PresentPosLo) ;
  255 + int result = dxl_get_result();
  256 + if( result != COMM_RXSUCCESS )
  257 + {
  258 + TxDString("\nproblem, code=");
  259 + TxDWord16(result);
  260 + TxDString("!!!\n");
  261 + }
  262 +}
  263 +
  264 +/////////////////////////////////////////////////////
  265 +////////////////////////////// AX S1 ////////////////
  266 +/////////////////////////////////////////////////////
  267 +
  268 +// returns the obstacle detection flag (using infrared sensors), see technical documentation
  269 +// parameter sensor: Id of AX-S1
  270 +// parameter boolLight: pointer to store data read from AX-S1
  271 +void checkObstacle(unsigned char sensor, unsigned char* infoObst) {
  272 + *infoObst = dxl_read_byte(sensor, AXS1_CTAB_ID_ObstacleDetectionFlag) ;
  273 + int result = dxl_get_result();
  274 + if( result != COMM_RXSUCCESS )
  275 + {
  276 + TxDString("\nproblem, code=");
  277 + TxDWord16(result);
  278 + TxDString("!!!\n");
  279 + }
  280 +
  281 +}
  282 +
  283 +
  284 +// returns the light detection flag (using visual light sensors), see technical documentation
  285 +// parameter sensor: Id of AX-S1
  286 +// parameter boolLight: pointer to store data read from AX-S1
  287 +void checkLuminosity(unsigned char sensor, unsigned char* info) {
  288 + *info = dxl_read_byte(sensor, AXS1_CTAB_ID_LuminosityDetectionFlag) ;
  289 + int result = dxl_get_result();
  290 + if( result != COMM_RXSUCCESS )
  291 + {
  292 + TxDString("\nproblem, code=");
  293 + TxDWord16(result);
  294 + TxDString("!!!\n");
  295 + }
  296 +
  297 +}
  298 +// returns the left infrared reading. Is a numerical value not just a flag!
  299 +// parameter sensor: Id of AX-S1
  300 +// parameter sideIR: pointer to store data read from AX-S1
  301 +void leftInfraRed(unsigned char sensor, unsigned char* info) {
  302 + *info = dxl_read_byte(sensor, AXS1_CTAB_ID_LeftIRSensorData) ;
  303 + int result = dxl_get_result();
  304 + if( result != COMM_RXSUCCESS )
  305 + {
  306 + TxDString("\nproblem, code=");
  307 + TxDWord16(result);
  308 + TxDString("!!!\n");
  309 + }
  310 +}
  311 +
  312 +// returns the center infrared reading. Is a numerical value not just a flag!
  313 +// parameter sensor: Id of AX-S1
  314 +// parameter sideIR: pointer to store data read from AX-S1
  315 +void centerInfraRed(unsigned char sensor, unsigned char* info) {
  316 + *info = dxl_read_byte(sensor, AXS1_CTAB_ID_CenterIRSensorData) ;
  317 + int result = dxl_get_result();
  318 + if( result != COMM_RXSUCCESS )
  319 + {
  320 + TxDString("\nproblem, code=");
  321 + TxDWord16(result);
  322 + TxDString("!!!\n");
  323 + }
  324 +}
  325 +
  326 +// returns the right infrared reading. Is a numerical value not just a flag!
  327 +// parameter sensor: Id of AX-S1
  328 +// parameter sideIR: pointer to store data read from AX-S1
  329 +void rightInfraRed(unsigned char sensor, unsigned char* info) {
  330 + *info = dxl_read_byte(sensor, AXS1_CTAB_ID_RightIRSensorData) ;
  331 + int result = dxl_get_result();
  332 + if( result != COMM_RXSUCCESS )
  333 + {
  334 + TxDString("\nproblem, code=");
  335 + TxDWord16(result);
  336 + TxDString("!!!\n");
  337 + }
  338 +}
  339 +
  340 +
  341 +// returns the left leight sensor reading. Is a numerical value not just a flag!
  342 +// parameter sensor: Id of AX-S1
  343 +// parameter leftLum: pointer to store data read from AX-S1
  344 +void leftLuminosity(unsigned char sensor, unsigned char* info) {
  345 + *info = dxl_read_byte(sensor,AXS1_CTAB_ID_LeftLuminosity ) ;
  346 + int result = dxl_get_result();
  347 + if( result != COMM_RXSUCCESS )
  348 + {
  349 + TxDString("\nproblem, code=");
  350 + TxDWord16(result);
  351 + TxDString("!!!\n");
  352 + }
  353 +}
  354 +
  355 +// returns the central light sensor reading. Is a numerical value not just a flag!
  356 +// parameter sensor: Id of AX-S1
  357 +// parameter centerLum: pointer to store data read from AX-S1
  358 +void centerLuminosity(unsigned char sensor, unsigned char* info) {
  359 + *info = dxl_read_byte(sensor,AXS1_CTAB_ID_CenterLuminosity ) ;
  360 + int result = dxl_get_result();
  361 + if( result != COMM_RXSUCCESS )
  362 + {
  363 + TxDString("\nproblem, code=");
  364 + TxDWord16(result);
  365 + TxDString("!!!\n");
  366 + }
  367 +}
  368 +
  369 +// returns the right light sensor reading. Is a numerical value not just a flag!
  370 +// parameter sensor: Id of AX-S1
  371 +// parameter rightLum: pointer to store data read from AX-S1
  372 +void rightLuminosity(unsigned char sensor, unsigned char* info) {
  373 + *info = dxl_read_byte(sensor,AXS1_CTAB_ID_RightLuminosity ) ;
  374 + int result = dxl_get_result();
  375 + if( result != COMM_RXSUCCESS )
  376 + {
  377 + TxDString("\nproblem, code=");
  378 + TxDWord16(result);
  379 + TxDString("!!!\n");
  380 + }
  381 +}
  382 +
  383 +
  384 +// returns the amount of sound detected
  385 +// untested, see documentation of AX-S1!!
  386 +void dataSound(unsigned char sensor, unsigned int* info) {
  387 + *info = dxl_read_word(sensor,AXS1_CTAB_ID_SoundData) ;
  388 + int result = dxl_get_result();
  389 + if( result != COMM_RXSUCCESS )
  390 + {
  391 + TxDString("\nproblem, code=");
  392 + TxDWord16(result);
  393 + TxDString("!!!\n");
  394 + }
  395 +}
  396 +
  397 +
  398 +//helper function
  399 +void noteBuzz(unsigned char sensor, int note) {
  400 + dxl_write_byte(sensor, AXS1_CTAB_ID_BuzzerIndex, note) ;
  401 + int result = dxl_get_result();
  402 + if( result != COMM_RXSUCCESS )
  403 + {
  404 + TxDString("\nproblem, code=");
  405 + TxDWord16(result);
  406 + TxDString("!!!\n");
  407 + }
  408 +}
  409 +
  410 +//helper function
  411 +void timeBuzz(unsigned char sensor, int time) {
  412 + int result=-1 ;
  413 + while (result != COMM_RXSUCCESS)
  414 + {
  415 + dxl_write_byte(sensor, AXS1_CTAB_ID_BuzzerTime, time) ;
  416 + result = dxl_get_result();
  417 + }
  418 + if( result != COMM_RXSUCCESS )
  419 + {
  420 + TxDString("\nproblem, code=");
  421 + TxDWord16(result);
  422 + TxDString("!!!\n");
  423 + }
  424 +}
  425 +
  426 +// play a note of given duration on the AX-S1.
  427 +// tim is in milliseconds, so 500 means half a second
  428 +void buzzWithDelay(unsigned char sensor, int note, int time) {
  429 + int k = 0;
  430 + // infinite duration buzz
  431 + timeBuzz(sensor,254);
  432 + noteBuzz(sensor, note);
  433 +
  434 + for (k=0; k<time; k++) {
  435 + mDelay(1) ;
  436 + }
  437 +
  438 + // shut off buzz
  439 + timeBuzz(sensor,0) ;
  440 + return ;
  441 +
  442 +}
  443 +
  444 +/////////////////////////////////////////////////////////////////////////
  445 +////////////////////////// M A I N L O O P ////////////////////////
  446 +/////////////////////////////////////////////////////////////////////////
  447 +
  448 +
  449 +int main(void)
  450 +{
  451 + // --------------DO NOT TOUCH!!------------------------ //
  452 + // NEVER!!! EVER!!!
  453 +
  454 + /* System Clocks Configuration */
  455 + RCC_Configuration();
  456 +
  457 + /* NVIC configuration */
  458 + NVIC_Configuration();
  459 +
  460 + /* GPIO configuration */
  461 + GPIO_Configuration();
  462 +
  463 + SysTick_Configuration();
  464 +
  465 + Timer_Configuration();
  466 +
  467 + dxl_initialize( 0, 1 );
  468 + USART_Configuration(USART_PC, Baudrate_PC);
  469 + // -----------------UNTIL HERE------------------------- //
  470 +
  471 + // here, touch all u like :-)
  472 + //
  473 + int thresholdInfrared = 50 ;
  474 + // state will define the robot's attitude towards its environment
  475 + // it implements a state machine
  476 + // thats why we call it state :-)
  477 + int state;
  478 +
  479 +
  480 + state= INIT;
  481 + infiniteTurn(MOTOR_up_left);
  482 + infiniteTurn(MOTOR_up_right);
  483 + infiniteTurn(MOTOR_down_left);
  484 + infiniteTurn(MOTOR_down_right);
  485 + unsigned char field;
  486 +
  487 + // printf("Resetting motors\n") ;
  488 + setSpeed(MOTOR_up_left, 0);
  489 + setSpeed(MOTOR_up_right, 0);
  490 + setSpeed(MOTOR_down_left, 0);
  491 + setSpeed(MOTOR_down_right, 0);
  492 +
  493 + // state should equal INIT only at the beginning of each match
  494 +
  495 + while(state!=STOP)
  496 + {
  497 +
  498 + while (state==INIT) {
  499 + int z = 0;
  500 + // play some music
  501 + buzzWithDelay(SENSOR, 30, 500);
  502 + buzzWithDelay(SENSOR, 40, 200);
  503 + buzzWithDelay(SENSOR, 50, 200);
  504 +
  505 + // blink some lights
  506 + TxDString("blink!!\n") ;
  507 + for(z=0; z<3; z++)
  508 + {
  509 +
  510 + GPIO_SetBits(PORT_LED_POWER, PIN_LED_POWER);
  511 + GPIO_ResetBits(PORT_LED_MANAGE, PIN_LED_MANAGE);
  512 + mDelay(250);
  513 +
  514 + GPIO_SetBits(PORT_LED_MANAGE, PIN_LED_MANAGE);
  515 + GPIO_ResetBits(PORT_LED_PROGRAM, PIN_LED_PROGRAM);
  516 + mDelay(250);
  517 +
  518 + GPIO_SetBits(PORT_LED_PROGRAM, PIN_LED_PROGRAM);
  519 + GPIO_ResetBits(PORT_LED_PLAY, PIN_LED_PLAY);
  520 + mDelay(250);
  521 +
  522 + GPIO_SetBits(PORT_LED_PLAY, PIN_LED_PLAY);
  523 + GPIO_ResetBits(PORT_LED_TX, PIN_LED_TX);
  524 + mDelay(250);
  525 +
  526 + GPIO_SetBits(PORT_LED_TX, PIN_LED_TX);
  527 + GPIO_ResetBits(PORT_LED_RX, PIN_LED_RX);
  528 + mDelay(250);
  529 +
  530 + GPIO_SetBits(PORT_LED_RX, PIN_LED_RX);
  531 + GPIO_ResetBits(PORT_LED_AUX, PIN_LED_AUX);
  532 + mDelay(250);
  533 +
  534 + GPIO_SetBits(PORT_LED_AUX, PIN_LED_AUX);
  535 + GPIO_ResetBits(PORT_LED_POWER, PIN_LED_POWER);
  536 + mDelay(250);
  537 +
  538 + /* TxDString("lights on...\n") ; */
  539 + /* lightOn(MOTOR_up_right); */
  540 + /* lightOn(MOTOR_up_left); */
  541 + /* lightOn(MOTOR_down_left); */
  542 + /* lightOn(MOTOR_down_right); */
  543 + /* mDelay(2) ; */
  544 + /* TxDString("lights oFF...\n") ; */
  545 + /* lightOff(MOTOR_up_right); */
  546 + /* lightOff(MOTOR_up_left); */
  547 + /* lightOff(MOTOR_down_left); */
  548 + /* lightOff(MOTOR_down_right); */
  549 + }
  550 +
  551 + state=GO_TO_CENTER;
  552 +
  553 + }
  554 +
  555 +
  556 + while (state==GO_TO_CENTER) { // the temporisation should be adapted
  557 + TxDString("\nGO TO CENTER\n") ;
  558 + // go straight assuming its a 4_wheeled robot to the center of the field
  559 + setSpeed(MOTOR_up_left, speed_ini);
  560 + setSpeed(MOTOR_down_left, speed_ini);
  561 + // /!\ since the motors are set in opposite directions, the speeds should
  562 + // be opposite for each side
  563 + setSpeed(MOTOR_up_right, -speed_ini);
  564 + setSpeed(MOTOR_down_right, -speed_ini);
  565 +
  566 + // advance for 3s, maybe adapt...
  567 + mDelay(3);
  568 + state=SEEKING;
  569 + }
  570 +
  571 +
  572 +
  573 + // begin the "seeking for an opponent" phase
  574 + while (state==SEEKING) {
  575 + // the robot starts spinning around
  576 + setSpeed(MOTOR_up_left, speed_ini);
  577 + setSpeed(MOTOR_up_right, speed_ini);
  578 + setSpeed(MOTOR_down_left, speed_ini);
  579 + setSpeed(MOTOR_down_right, speed_ini);
  580 + centerInfraRed(SENSOR, &field);
  581 + {
  582 + TxDString("\nSEEKING SENSOR VALUE") ;
  583 + TxDByte16(field);
  584 + TxDString("\n") ;
  585 + }
  586 +
  587 + // opponent detection will result in an attitude change
  588 + if (field >= thresholdInfrared) // indeed this condition should be explicit
  589 + state = CHASING;
  590 +
  591 + }
  592 +
  593 + // the robot will focus the opponent and try to push him away,
  594 + // as hard as possible
  595 + while (state==CHASING) {
  596 + setSpeed(MOTOR_up_left, speed_max);
  597 + setSpeed(MOTOR_down_left, speed_max);
  598 + setSpeed(MOTOR_up_right, -speed_max);
  599 + setSpeed(MOTOR_down_right, -speed_max);
  600 + centerInfraRed(SENSOR, &field);
  601 + { TxDString("\nCHASING SENSOR VALUE") ;
  602 + TxDByte16(field);
  603 + TxDString("\n") ;
  604 + }
  605 +
  606 + // if, for whatever reason, the robot does not detect any obstacle anymore
  607 + // it returns to its seeking opponent phase
  608 + if (field<thresholdInfrared/2)
  609 + state=SEEKING;
  610 +
  611 +
  612 + }
  613 +
  614 + }
  615 +
  616 + while (1) {} ;
  617 +
  618 + return 0;
  619 +}
  620 +
  621 +// --------------DO NOT TOUCH!!------------------------ //
  622 +// NEVER!!! EVER!!!
  623 +
  624 +/*******************************************************************************
  625 + * Function Name : RCC_Configuration
  626 + * Description : Configures the different system clocks.
  627 + * Input : None
  628 + * Output : None
  629 + * Return : None
  630 + *******************************************************************************/
  631 +void RCC_Configuration(void)
  632 +{
  633 + ErrorStatus HSEStartUpStatus;
  634 + /* RCC system reset(for debug purpose) */
  635 + RCC_DeInit();
  636 +
  637 + /* Enable HSE */
  638 + RCC_HSEConfig(RCC_HSE_ON);
  639 +
  640 + /* Wait till HSE is ready */
  641 + HSEStartUpStatus = RCC_WaitForHSEStartUp();
  642 +
  643 + if(HSEStartUpStatus == SUCCESS)
  644 + {
  645 + /* Enable Prefetch Buffer */
  646 + FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
  647 +
  648 + /* Flash 2 wait state */
  649 + FLASH_SetLatency(FLASH_Latency_2);
  650 +
  651 + /* HCLK = SYSCLK */
  652 + RCC_HCLKConfig(RCC_SYSCLK_Div1);
  653 +
  654 + /* PCLK2 = HCLK */
  655 + RCC_PCLK2Config(RCC_HCLK_Div1);
  656 +
  657 + /* PCLK1 = HCLK/2 */
  658 + RCC_PCLK1Config(RCC_HCLK_Div2);
  659 +
  660 + /* PLLCLK = 8MHz * 9 = 72 MHz */
  661 + RCC_PLLConfig(RCC_PLLSource_HSE_Div1, RCC_PLLMul_9);
  662 +
  663 + /* Enable PLL */
  664 + RCC_PLLCmd(ENABLE);
  665 +
  666 + /* Wait till PLL is ready */
  667 + while(RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == RESET)
  668 + {
  669 + }
  670 +
  671 + /* Select PLL as system clock source */
  672 + RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);
  673 +
  674 + /* Wait till PLL is used as system clock source */
  675 + while(RCC_GetSYSCLKSource() != 0x08)
  676 + {
  677 + }
  678 + }
  679 +
  680 + /* Enable peripheral clocks --------------------------------------------------*/
  681 +
  682 + /* Enable USART1 and GPIOB clocks */
  683 + RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1 | RCC_APB2Periph_GPIOB, ENABLE);
  684 +
  685 + /* Enable USART3 clocks */
  686 + RCC_APB1PeriphClockCmd ( RCC_APB1Periph_USART3 | RCC_APB1Periph_TIM2, ENABLE);
  687 +
  688 + PWR_BackupAccessCmd(ENABLE);
  689 +}
  690 +
  691 +/*******************************************************************************
  692 + * Function Name : NVIC_Configuration
  693 + * Description : Configures Vector Table base location.
  694 + * Input : None
  695 + * Output : None
  696 + * Return : None
  697 + *******************************************************************************/
  698 +void NVIC_Configuration(void)
  699 +{
  700 + NVIC_InitTypeDef NVIC_InitStructure;
  701 +
  702 +#ifdef VECT_TAB_RAM
  703 + // Set the Vector Table base location at 0x20000000
  704 + NVIC_SetVectorTable(NVIC_VectTab_RAM, 0x0);
  705 +#else // VECT_TAB_FLASH
  706 + // Set the Vector Table base location at 0x08003000
  707 + NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x3000);
  708 +#endif
  709 +
  710 + // Configure the NVIC Preemption Priority Bits
  711 + NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
  712 +
  713 + // Enable the USART1 Interrupt
  714 + NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQChannel;
  715 + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
  716 + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
  717 + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  718 + NVIC_Init(&NVIC_InitStructure);
  719 +
  720 + // Enable the TIM2 Interrupt
  721 + NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQChannel;
  722 + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
  723 + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
  724 + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  725 + NVIC_Init(&NVIC_InitStructure);
  726 +}
  727 +
  728 +/*******************************************************************************
  729 + * Function Name : GPIO_Configuration
  730 + * Description : Configures the different GPIO ports.
  731 + * Input : None
  732 + * Output : None
  733 + * Return : None
  734 + *******************************************************************************/
  735 +void GPIO_Configuration(void)
  736 +{
  737 + GPIO_InitTypeDef GPIO_InitStructure;
  738 + GPIO_StructInit(&GPIO_InitStructure);
  739 +
  740 + // PORTB CONFIG
  741 + GPIO_InitStructure.GPIO_Pin = PIN_ENABLE_TXD | PIN_ENABLE_RXD;
  742 + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  743 + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  744 + GPIO_Init(GPIOB, &GPIO_InitStructure);
  745 +
  746 + GPIO_InitStructure.GPIO_Pin = PIN_DXL_RXD | PIN_PC_RXD;
  747 + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  748 + GPIO_Init(GPIOB, &GPIO_InitStructure);
  749 +
  750 + GPIO_InitStructure.GPIO_Pin = PIN_DXL_TXD | PIN_PC_TXD;
  751 + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  752 + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  753 + GPIO_Init(GPIOB, &GPIO_InitStructure);
  754 +
  755 + GPIO_PinRemapConfig( GPIO_Remap_USART1, ENABLE);
  756 + GPIO_PinRemapConfig( GPIO_Remap_SWJ_Disable, ENABLE);
  757 +
  758 + GPIO_ResetBits(PORT_ENABLE_TXD, PIN_ENABLE_TXD); // TX Disable
  759 + GPIO_SetBits(PORT_ENABLE_RXD, PIN_ENABLE_RXD); // RX Enable
  760 +}
  761 +
  762 +void USART1_Configuration(u32 baudrate)
  763 +{
  764 + USART_Configuration(USART_DXL, baudrate);
  765 +}
  766 +
  767 +void USART_Configuration(u8 PORT, u32 baudrate)
  768 +{
  769 +
  770 + USART_InitTypeDef USART_InitStructure;
  771 +
  772 + USART_StructInit(&USART_InitStructure);
  773 +
  774 +
  775 + USART_InitStructure.USART_BaudRate = baudrate;
  776 + USART_InitStructure.USART_WordLength = USART_WordLength_8b;
  777 + USART_InitStructure.USART_StopBits = USART_StopBits_1;
  778 + USART_InitStructure.USART_Parity = USART_Parity_No ;
  779 + USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
  780 + USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
  781 +
  782 +
  783 + if( PORT == USART_DXL )
  784 + {
  785 + USART_DeInit(USART1);
  786 + mDelay(10);
  787 + /* Configure the USART1 */
  788 + USART_Init(USART1, &USART_InitStructure);
  789 +
  790 + /* Enable USART1 Receive and Transmit interrupts */
  791 + USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
  792 + //USART_ITConfig(USART1, USART_IT_TC, ENABLE);
  793 +
  794 + /* Enable the USART1 */
  795 + USART_Cmd(USART1, ENABLE);
  796 + }
  797 +
  798 + else if( PORT == USART_PC )
  799 + {
  800 + USART_DeInit(USART3);
  801 + mDelay(10);
  802 + /* Configure the USART3 */
  803 + USART_Init(USART3, &USART_InitStructure);
  804 +
  805 + /* Enable USART3 Receive and Transmit interrupts */
  806 + //USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);
  807 + //USART_ITConfig(USART3, USART_IT_TC, ENABLE);
  808 +
  809 + /* Enable the USART3 */
  810 + USART_Cmd(USART3, ENABLE);
  811 + }
  812 +}
  813 +
  814 +void DisableUSART1(void)
  815 +{
  816 + USART_Cmd(USART1, DISABLE);
  817 +}
  818 +
  819 +void ClearBuffer256(void)
  820 +{
  821 + gbRxBufferReadPointer = gbRxBufferWritePointer = 0;
  822 +}
  823 +
  824 +byte CheckNewArrive(void)
  825 +{
  826 + if(gbRxBufferReadPointer != gbRxBufferWritePointer)
  827 + return 1;
  828 + else
  829 + return 0;
  830 +}
  831 +
  832 +void TxDByte_DXL(byte bTxdData)
  833 +{
  834 + GPIO_ResetBits(PORT_ENABLE_RXD, PIN_ENABLE_RXD); // RX Disable
  835 + GPIO_SetBits(PORT_ENABLE_TXD, PIN_ENABLE_TXD); // TX Enable
  836 +
  837 + USART_SendData(USART1,bTxdData);
  838 + while( USART_GetFlagStatus(USART1, USART_FLAG_TC)==RESET );
  839 +
  840 + GPIO_ResetBits(PORT_ENABLE_TXD, PIN_ENABLE_TXD); // TX Disable
  841 + GPIO_SetBits(PORT_ENABLE_RXD, PIN_ENABLE_RXD); // RX Enable
  842 +}
  843 +
  844 +byte RxDByte_DXL(void)
  845 +{
  846 + byte bTemp;
  847 +
  848 + while(1)
  849 + {
  850 + if(gbRxBufferReadPointer != gbRxBufferWritePointer) break;
  851 + }
  852 +
  853 + bTemp = gbpRxInterruptBuffer[gbRxBufferReadPointer];
  854 + gbRxBufferReadPointer++;
  855 +
  856 + return bTemp;
  857 +}
  858 +
  859 +
  860 +// Print communication result
  861 +void PrintCommStatus(int CommStatus)
  862 +{
  863 + switch(CommStatus)
  864 + {
  865 + case COMM_TXFAIL:
  866 + TxDString("COMM_TXFAIL: Failed transmit instruction packet!\n");
  867 + break;
  868 +
  869 + case COMM_TXERROR:
  870 + TxDString("COMM_TXERROR: Incorrect instruction packet!\n");
  871 + break;
  872 +
  873 + case COMM_RXFAIL:
  874 + TxDString("COMM_RXFAIL: Failed get status packet from device!\n");
  875 + break;
  876 +
  877 + case COMM_RXWAITING:
  878 + TxDString("COMM_RXWAITING: Now recieving status packet!\n");
  879 + break;
  880 +
  881 + case COMM_RXTIMEOUT:
  882 + TxDString("COMM_RXTIMEOUT: There is no status packet!\n");
  883 + break;
  884 +
  885 + case COMM_RXCORRUPT:
  886 + TxDString("COMM_RXCORRUPT: Incorrect status packet!\n");
  887 + break;
  888 +
  889 + default:
  890 + TxDString("This is unknown error code!\n");
  891 + break;
  892 + }
  893 +}
  894 +
  895 +// Print error bit of status packet
  896 +void PrintErrorCode()
  897 +{
  898 + if(dxl_get_rxpacket_error(ERRBIT_VOLTAGE) == 1)
  899 + TxDString("Input voltage error!\n");
  900 +
  901 + if(dxl_get_rxpacket_error(ERRBIT_ANGLE) == 1)
  902 + TxDString("Angle limit error!\n");
  903 +
  904 + if(dxl_get_rxpacket_error(ERRBIT_OVERHEAT) == 1)
  905 + TxDString("Overheat error!\n");
  906 +
  907 + if(dxl_get_rxpacket_error(ERRBIT_RANGE) == 1)
  908 + TxDString("Out of range error!\n");
  909 +
  910 + if(dxl_get_rxpacket_error(ERRBIT_CHECKSUM) == 1)
  911 + TxDString("Checksum error!\n");
  912 +
  913 + if(dxl_get_rxpacket_error(ERRBIT_OVERLOAD) == 1)
  914 + TxDString("Overload error!\n");
  915 +
  916 + if(dxl_get_rxpacket_error(ERRBIT_INSTRUCTION) == 1)
  917 + TxDString("Instruction code error!\n");
  918 +}
  919 +
  920 +void TxDString(byte *bData)
  921 +{
  922 + while (*bData)
  923 + TxDByte_PC(*bData++);
  924 +}
  925 +
  926 +void TxDWord16(word wSentData)
  927 +{
  928 + TxDByte16((wSentData >> 8) & 0xff);
  929 + TxDByte16(wSentData & 0xff);
  930 +}
  931 +
  932 +void TxDByte16(byte bSentData)
  933 +{
  934 + byte bTmp;
  935 +
  936 + bTmp = ((byte) (bSentData >> 4) & 0x0f) + (byte) '0';
  937 + if (bTmp > '9')
  938 + bTmp += 7;
  939 + TxDByte_PC(bTmp);
  940 + bTmp = (byte) (bSentData & 0x0f) + (byte) '0';
  941 + if (bTmp > '9')
  942 + bTmp += 7;
  943 + TxDByte_PC(bTmp);
  944 +}
  945 +
  946 +void TxDByte_PC(byte bTxdData)
  947 +{
  948 + USART_SendData(USART3,bTxdData);
  949 + while( USART_GetFlagStatus(USART3, USART_FLAG_TC)==RESET );
  950 +}
  951 +
  952 +void Timer_Configuration(void)
  953 +{
  954 + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
  955 + TIM_OCInitTypeDef TIM_OCInitStructure;
  956 +
  957 + TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
  958 + TIM_OCStructInit(&TIM_OCInitStructure);
  959 +
  960 + TIM_DeInit(TIM2);
  961 +
  962 + /* Time base configuration */
  963 + TIM_TimeBaseStructure.TIM_Period = 65535;
  964 + TIM_TimeBaseStructure.TIM_Prescaler = 0;
  965 + TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  966 + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
  967 +
  968 + TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
  969 +
  970 + /* Prescaler configuration */
  971 + TIM_PrescalerConfig(TIM2, 722, TIM_PSCReloadMode_Immediate);
  972 +
  973 + /* Output Compare Timing Mode configuration: Channel1 */
  974 + TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing;
  975 + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable;
  976 + TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
  977 + TIM_OCInitStructure.TIM_Pulse = CCR1_Val ;
  978 +
  979 + TIM_OC1Init(TIM2, &TIM_OCInitStructure);
  980 + TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Disable);
  981 +
  982 + /* TIM IT enable */
  983 + TIM_ITConfig(TIM2, TIM_IT_CC1, ENABLE);
  984 +
  985 + /* TIM2 enable counter */
  986 + TIM_Cmd(TIM2, ENABLE);
  987 +}
  988 +
  989 +void TimerInterrupt_1ms(void) //OLLO CONTROL
  990 +{
  991 + if (TIM_GetITStatus(TIM2, TIM_IT_CC1) != RESET) // 1ms//