; 18-649 Fall 2015 ; ; Group 12 ; --------------------------- ; Daniel Gorziglia - dgorzigl ; Shepard Emerson - semerson ; Tom Eliot - tke ; Daniel Haddox - dhaddox ; ; Drive Control Unit Test 1 #INCLUDE defines.mf ;Initialize level sensors to True 0s I 0s N LEVEL_SENSOR_[UP]_CAN_ID Leveling UP = true 0s I 0s N LEVEL_SENSOR_[DOWN]_CAN_ID Leveling DOWN = true ;initialize at first floor 0s I 0s N AT_FLOOR_[1][FRONT]_CAN_ID AtFloor 1 FRONT = true ;init door motor commands to stop 0s I 0s N DOOR_MOTOR_COMMAND_[FRONT][RIGHT]_CAN_ID DoorMotor FRONT RIGHT = STOP 0s I 0s N DOOR_MOTOR_COMMAND_[FRONT][LEFT]_CAN_ID DoorMotor FRONT LEFT = STOP 0s I 0s N DOOR_MOTOR_COMMAND_[BACK][RIGHT]_CAN_ID DoorMotor BACK RIGHT = STOP 0s I 0s N DOOR_MOTOR_COMMAND_[BACK][LEFT]_CAN_ID DoorMotor BACK LEFT = STOP ; ------------------ ; State 1: Stop, Init ; ------------------ ;#state '6.S.1' ;Check Drive Physical message 0.3s A F Drive : speed == STOP 0.3s A F Drive : direction == STOP ;Check mDrive Network messages 0.3s A N DRIVE_CAN_ID Drive : getCommandDirection == STOP 0.3s A N DRIVE_CAN_ID Drive : getCommandSpeed == STOP ;Check mDriveSpeed Network messages, init to stop,0 so no need to inject 0.3s A N DRIVE_CAN_ID Drive : getSensorDirection == STOP 0.3s A N DRIVE_CAN_ID Drive : getSensorSpeed == 0 ; ------------------ ; Transition 6.T.1 ; ------------------ ; Set conditions for 6.T.1 ;#transition '6.T.1' ;set emergencybrake to off 0.4s I 0s N EMERGENCY_BRAKE_CAN_ID SafetySensor = false ;set drivespeed to stop 0.4s I 0s F DriveSpeed = STOP 0 ;set desiredDirection by setting atfloor true and mdesiredfloor up 0.4s I 0s N AT_FLOOR_[1][FRONT]_CAN_ID AtFloor 1 FRONT = true 0.4s I 0s N DESIRED_FLOOR_CAN_ID DesiredFloor = 2 STOP BACK ;set all door closed sensors to true 0.4s I 0s N DOOR_CLOSED_SENSOR_[FRONT][RIGHT]_CAN_ID DoorClosed FRONT RIGHT = true 0.4s I 0s N DOOR_CLOSED_SENSOR_[FRONT][LEFT]_CAN_ID DoorClosed FRONT LEFT = true 0.4s I 0s N DOOR_CLOSED_SENSOR_[BACK][RIGHT]_CAN_ID DoorClosed BACK RIGHT = true 0.4s I 0s N DOOR_CLOSED_SENSOR_[BACK][LEFT]_CAN_ID DoorClosed BACK LEFT = true ;set carweight to 1 < max 0.4s I 0s N CAR_WEIGHT_CAN_ID CarWeight = 1 ; ------------------ ; State 2: Level, Up ; ------------------ ;#state '6.S.2' ;Check Drive Physical message 1.0s A F Drive : speed == LEVEL 1.0s A F Drive : direction == UP ;Check mDrive Network messages 1.0s A N DRIVE_CAN_ID Drive : getCommandSpeed == LEVEL 1.0s A N DRIVE_CAN_ID Drive : getCommandDirection == UP ;Check mDriveSpeed Network messages, set physical sensor 1.0s I 0s F DriveSpeed = UP .01 1.1s A N DRIVE_CAN_ID Drive : getSensorDirection == UP 1.1s A N DRIVE_CAN_ID Drive : getSensorSpeed == .01 ; ------------------ ; Transition 6.T.2 ; ------------------ ; Set conditions for 6.T.2 ;#transition '6.T.2' ; carweight already set to < max ; all doors already closed ; emergency brake is off ; Set drivespeed to leveling speed 1.1s I 0s F DriveSpeed = UP 0.05 ; desired direciton is up ; ------------------ ; State 3: Slow, Up ; ------------------ ;#state '6.S.3' ;Check Drive Physical message 1.2s A F Drive : speed == SLOW 1.2s A F Drive : direction == UP ;Check mDrive Network messages 1.2s A N DRIVE_CAN_ID Drive : getCommandSpeed == SLOW 1.2s A N DRIVE_CAN_ID Drive : getCommandDirection == UP ;Check mDriveSpeed Network messages 1.2s A N DRIVE_CAN_ID Drive : getSensorDirection == UP 1.2s A N DRIVE_CAN_ID Drive : getSensorSpeed == .05 ; ------------------ ; Transition 6.T.3 ; ------------------ ; Set conditions for 6.T.3 ;#transition '6.T.3' ; emergency brake is off ; Set drivespeed to slow speed 1.2s I 0s F DriveSpeed = UP 0.25 ; ------------------ ; State 4: Fast, Up ; ------------------ ;#state '6.S.4' ;Check Drive Physical message 1.3s A F Drive : speed == FAST 1.3s A F Drive : direction == UP ;Check mDrive Network messages 1.3s A N DRIVE_CAN_ID Drive : getCommandSpeed == FAST 1.3s A N DRIVE_CAN_ID Drive : getCommandDirection == UP ;Check mDriveSpeed Network messages 1.3s A N DRIVE_CAN_ID Drive : getSensorDirection == UP 1.3s A N DRIVE_CAN_ID Drive : getSensorSpeed == .25 ; ------------------ ; Transition 6.T.4 ; ------------------ ; Set conditions for 6.T.4 ;#transition '6.T.4' ; emergency brake is off ; Set physical state for commit point calculation 1.3s I 0s F DriveSpeed = UP 0.30 ;Set physical state for commit point calculation 1.3s I 0s N CAR_LEVEL_POSITION_CAN_ID Integer = 04900 ; ------------------ ; State 3: Slow, Up ; ------------------ ;#state '6.S.3' ;Check Drive Physical message 1.35s A F Drive : speed == SLOW 1.35s A F Drive : direction == UP ;Check mDrive Network messages 1.35s A N DRIVE_CAN_ID Drive : getCommandSpeed == SLOW 1.35s A N DRIVE_CAN_ID Drive : getCommandDirection == UP ;Check mDriveSpeed Network messages 1.35s A N DRIVE_CAN_ID Drive : getSensorDirection == UP 1.35s A N DRIVE_CAN_ID Drive : getSensorSpeed == .3 ; ------------------ ; Transition 6.T.5 ; ------------------ ; Set conditions for 6.T.5 ;#transition '6.T.5' ; emergency brake is off ; set physical state to reach floor 1.35s I 0s F DriveSpeed = UP 0.25 ; set levelup so dont transition to stop 1.35s I 0s N LEVEL_SENSOR_[UP]_CAN_ID Leveling UP = false 1.35s I 0s N AT_FLOOR_[1][FRONT]_CAN_ID AtFloor 1 FRONT = false 1.37s I 0s N AT_FLOOR_[2][BACK]_CAN_ID AtFloor 2 BACK = true ; ------------------ ; State 2: Level, Up ; ------------------ ;#state '6.S.2' ;Check Drive Physical message 1.42s A F Drive : speed == LEVEL 1.42s A F Drive : direction == UP ;Check mDrive Network messages 1.42s A N DRIVE_CAN_ID Drive : getCommandSpeed == LEVEL 1.42s A N DRIVE_CAN_ID Drive : getCommandDirection == UP ;Check mDriveSpeed Network messages 1.42s A N DRIVE_CAN_ID Drive : getSensorDirection == UP 1.42s A N DRIVE_CAN_ID Drive : getSensorSpeed == .25 ; ------------------ ; Transition 6.T.6 ; ------------------ ; Set conditions for 6.T.6 ;#transition '6.T.6' ; emergency brake is off ; set level sensor 1.44s I 0s N LEVEL_SENSOR_[UP]_CAN_ID Leveling UP = true 1.44s I 0s F DriveSpeed = STOP 0 ; ------------------ ; State 1: Stop, Init ; ------------------ ;#state '6.S.1' ;Check Drive Physical message 1.7s A F Drive : speed == STOP 1.7s A F Drive : direction == STOP ;Check mDrive Network messages 1.7s A N DRIVE_CAN_ID Drive : getCommandDirection == STOP 1.7s A N DRIVE_CAN_ID Drive : getCommandSpeed == STOP ;Check mDriveSpeed Network messages, init to stop,0 so no need to inject 1.7s A N DRIVE_CAN_ID Drive : getSensorDirection == STOP 1.7s A N DRIVE_CAN_ID Drive : getSensorSpeed == 0