Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /************************************************************************
- Chip model : ATmega64L
- Clock Frequency : 8000000Mhz
- Project : Explorer 2
- Author : Vahe Danielyan(DanieV)
- Copyright (C) Vahe Danielyan <danielyan.vahe@gmail.com>
- *************************************************************************/
- #include "Logic.h"
- #include "buzzer.h"
- #include "lcd.h"
- #define LOGIC_START_DELAY_MSEC 100
- /* Line Following modes */
- UINT8 curLineMode;
- UINT8 newLineMode;
- /* Cubic Detecting modes */
- UINT8 modecd;
- /* Slimline Detecting modes */
- UINT8 modesld;
- /* Main Sensor Reading Variable */
- SENS_VALS Sensors;
- /* Main Motor Controlling Variable */
- MOTOR_MOVE_CTRL Motors;
- /* Motor Modes */
- UINT8 forw = MOTOR_FORWARD;
- UINT8 back = MOTOR_BACKWARD;
- UINT8 off = MOTOR_STOP;
- UINT8 last = MOTOR_DIR_LAST;
- TIMER_HANDLE logic_timer_id;
- #define LEFT_CORRECTION_SPEED MOTOR_LEFT_MAX_SPEED - 25 //(MOTOR_RIGHT_MAX_SPEED - 15)
- #define RIGHT_CORRECTION_SPEED MOTOR_RIGHT_MAX_SPEED - 25 //(MOTOR_LEFT_MAX_SPEED - 15)
- UINT16 loutrspeed = LEFT_CORRECTION_SPEED;
- UINT16 routlspeed = RIGHT_CORRECTION_SPEED;
- UINT16 motorspeedl = MOTOR_LEFT_MAX_SPEED;
- UINT16 motorspeedr = MOTOR_RIGHT_MAX_SPEED;
- void Start_logic();
- void Line_Follow();
- void Cubic_Detect();
- void SlimLine_Detect();
- void Process_Make_Desicions();
- void Wait_for_start();
- void Line_Follow()
- {
- {
- if(Sensors.LINE_L && Sensors.LINE_R)
- {
- newLineMode = LINE_TRACK_NORMAL;
- }
- else
- {
- if(!Sensors.LINE_L && !Sensors.LINE_R)
- {
- newLineMode = LINE_TRACK_LINE_OUT;
- }
- else
- {
- if(!Sensors.LINE_L)
- {
- newLineMode = LINE_TRACK_LOUT;
- }
- else
- {
- if(!Sensors.LINE_R)
- {
- newLineMode = LINE_TRACK_ROUT;
- }
- }
- }
- }
- }
- }
- void Process_Make_Desicions()
- {
- MOTOR_SPEED leftSpeed, rightSpeed;
- MOTOR_DIR leftDir, rightDir;
- //if(curLineMode != LINE_TRACK_OFF)
- {
- Motors.leftMotor.direction = NULL;
- Motors.rightMotor.direction = NULL;
- Motors.rightMotor.speed = NULL;
- Motors.leftMotor.speed = NULL;
- if (newLineMode != curLineMode) {
- leftDir = MOTOR_FORWARD;
- rightDir = MOTOR_FORWARD;
- Motors.leftMotor.direction = &leftDir;
- Motors.rightMotor.direction = &rightDir;
- switch (newLineMode) {
- case LINE_TRACK_NORMAL:
- leftSpeed = MOTOR_LEFT_MAX_SPEED;
- rightSpeed = MOTOR_RIGHT_MAX_SPEED;
- Motors.leftMotor.speed = &leftSpeed;
- Motors.rightMotor.speed = &rightSpeed;
- break;
- case LINE_TRACK_LOUT:
- leftSpeed = MOTOR_LEFT_MAX_SPEED;
- rightSpeed = RIGHT_CORRECTION_SPEED;
- Motors.rightMotor.speed = &rightSpeed;
- Motors.leftMotor.speed = &leftSpeed;
- break;
- case LINE_TRACK_ROUT:
- rightSpeed = MOTOR_RIGHT_MAX_SPEED;
- leftSpeed = LEFT_CORRECTION_SPEED;
- Motors.leftMotor.speed = &leftSpeed;
- Motors.rightMotor.speed = &rightSpeed;
- break;
- case LINE_TRACK_LINE_OUT:
- if(curLineMode == LINE_TRACK_LOUT)
- {
- leftSpeed = MOTOR_LEFT_MAX_SPEED;
- rightSpeed = RIGHT_CORRECTION_SPEED - 20;
- Motors.rightMotor.speed = &rightSpeed;
- Motors.leftMotor.speed = &leftSpeed;
- }
- else
- if(curLineMode == LINE_TRACK_ROUT)
- {
- rightSpeed = MOTOR_RIGHT_MAX_SPEED;
- leftSpeed = LEFT_CORRECTION_SPEED - 20;
- Motors.leftMotor.speed = &leftSpeed;
- Motors.rightMotor.speed = &rightSpeed;
- }
- break;
- default:
- ASSERT(0);
- break;
- }
- curLineMode = newLineMode;
- }
- motor_ctrl(Motors);
- }
- }
- void Start_logic(KNL_JOB_ARG bleh)
- {
- if(!BSP_GET_BUTN_VAL)
- {
- Read_Sensors(&Sensors);
- // Cubic_Detect();
- // SlimLine_Detect();
- Line_Follow();
- Process_Make_Desicions();
- logic_timer_id = knl_addTimer(Start_logic, LOGIC_SAMPLING_PERIOD_MSEC, TIMER_ONESHOT_CB,0);
- }
- else
- {
- Motors.leftMotor.direction = &off;
- Motors.rightMotor.direction = &off;
- Motors.leftMotor.speed = NULL;
- Motors.rightMotor.speed = NULL;
- motor_ctrl(Motors);
- curLineMode = LINE_TRACK_OFF;
- newLineMode = curLineMode;
- BSP_LED2_ON();
- knl_stopTimer(&logic_timer_id);
- knl_addJob(Logic_start, 0);
- buzzer_start(BUZZER_PATTERN_SINGLE_LONG);
- }
- }
- void Wait_for_start(KNL_JOB_ARG arg)
- {
- if(!BSP_GET_BUTN_VAL)
- {
- knl_stopTimer(&logic_timer_id);
- logic_timer_id = knl_addTimer(Start_logic, LOGIC_START_DELAY_MSEC, TIMER_ONESHOT_CB, 0);
- Motors.leftMotor.direction = &forw;
- Motors.rightMotor.direction = &forw;
- Motors.leftMotor.speed = &motorspeedl;
- Motors.rightMotor.speed = &motorspeedr;
- motor_ctrl(Motors);
- curLineMode = LINE_TRACK_NORMAL;
- newLineMode = curLineMode;
- BSP_LED2_OFF();
- buzzer_start(BUZZER_PATTERN_SINGLE_SHORT);
- }
- else {
- arg++;
- if(arg == 5)
- {
- lcd_printf(2,"%u%u%u%u%u%u",BSP_SENS1_VAL,BSP_SENS2_VAL,BSP_SENS3_VAL,BSP_SENS4_VAL,BSP_SENS5_VAL,BSP_SENS6_VAL);
- arg = 0;
- }
- logic_timer_id = knl_addTimer(Wait_for_start, LOGIC_START_DELAY_MSEC, TIMER_ONESHOT_JOB, arg);
- }
- }
- void Logic_start(KNL_JOB_ARG arg)
- {
- curLineMode = LINE_TRACK_OFF;
- newLineMode = curLineMode;
- logic_timer_id = knl_addTimer(Wait_for_start, LOGIC_START_DELAY_MSEC, TIMER_ONESHOT_JOB, 0);
- BSP_LED2_ON();
- }
- void Logic_init()
- {
- Sensor_init();
- motor_init();
- BSP_START_BUTTON_INIT();
- Motors.leftMotor.direction = &off;
- Motors.rightMotor.direction = &off;
- Motors.leftMotor.speed = NULL;
- Motors.rightMotor.speed = NULL;
- motor_ctrl(Motors);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement