Jordan Brack / SmartWheelsPublic

SmartWheels self-driving race car. Designed for NXP Cup. Uses FRDM-KL25Z, area-scan camera, and simple image processing to detect and navigate any NXP spec track.

Dependencies:   TSI USBDevice mbed-dev

Fork of SmartWheels by haofan Zheng

StateMachine/RunningState.cpp@80:c85cb93713b3, 2017-04-18 (annotated)

Committer:
hazheng
Date:
Tue Apr 18 19:26:33 2017 +0000
Branch:
Drift
Revision:
80:c85cb93713b3
Child:
81:32bd7a25a699
Transfer to State Machine.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hazheng 80:c85cb93713b3 1 #include "RunningState.h"
hazheng 80:c85cb93713b3 2
hazheng 80:c85cb93713b3 3 #include "StateManager.h"
hazheng 80:c85cb93713b3 4
hazheng 80:c85cb93713b3 5 #include "ArduCAM.h"
hazheng 80:c85cb93713b3 6 #include "ArduUTFT.h"
hazheng 80:c85cb93713b3 7 #include "Motor.h"
hazheng 80:c85cb93713b3 8 #include "Servo.h"
hazheng 80:c85cb93713b3 9
hazheng 80:c85cb93713b3 10 #define PI 3.14159265f
hazheng 80:c85cb93713b3 11
hazheng 80:c85cb93713b3 12 RunningState::RunningState() :
hazheng 80:c85cb93713b3 13 m_speedRatio(1.0f),
hazheng 80:c85cb93713b3 14 m_lastAngle(0.0f),
hazheng 80:c85cb93713b3 15 m_lastAngleAbs(0.0f),
hazheng 80:c85cb93713b3 16 m_cornerRatio(1.0f),
hazheng 80:c85cb93713b3 17 m_lastLineFound(BOTH_FOUND)
hazheng 80:c85cb93713b3 18 {}
hazheng 80:c85cb93713b3 19
hazheng 80:c85cb93713b3 20 RunningState::~RunningState()
hazheng 80:c85cb93713b3 21 {}
hazheng 80:c85cb93713b3 22
hazheng 80:c85cb93713b3 23 void RunningState::DrawUserInterface()
hazheng 80:c85cb93713b3 24 {
hazheng 80:c85cb93713b3 25
hazheng 80:c85cb93713b3 26 }
hazheng 80:c85cb93713b3 27
hazheng 80:c85cb93713b3 28 void RunningState::Update(float deltaTime)
hazheng 80:c85cb93713b3 29 {
hazheng 80:c85cb93713b3 30 image_processing();
hazheng 80:c85cb93713b3 31 uint8_t shouldTerminate = ardu_cam_get_is_encounter_terminate();
hazheng 80:c85cb93713b3 32
hazheng 80:c85cb93713b3 33 if(shouldTerminate)
hazheng 80:c85cb93713b3 34 {
hazheng 80:c85cb93713b3 35 TerminateProcess();
hazheng 80:c85cb93713b3 36 return;
hazheng 80:c85cb93713b3 37 }
hazheng 80:c85cb93713b3 38 //else
hazheng 80:c85cb93713b3 39 //{
hazheng 80:c85cb93713b3 40 // ardu_utft_print(" ", 230, 100);
hazheng 80:c85cb93713b3 41 //}
hazheng 80:c85cb93713b3 42
hazheng 80:c85cb93713b3 43 const volatile uint8_t* centerLine = ardu_cam_get_center_array();
hazheng 80:c85cb93713b3 44
hazheng 80:c85cb93713b3 45 /////////////Calculate the curvature:
hazheng 80:c85cb93713b3 46 float srcY = (0.0f + 1.0f) / 2.0f;
hazheng 80:c85cb93713b3 47 float srcX = static_cast<float>(centerLine[0] + centerLine[1]) / 2.0f;
hazheng 80:c85cb93713b3 48
hazheng 80:c85cb93713b3 49 float destY = static_cast<float>(CAM_ROI_UPPER_LIMIT - 1 + (CAM_ROI_UPPER_LIMIT - 2)) / 2.0f;
hazheng 80:c85cb93713b3 50 float destX = static_cast<float>(centerLine[CAM_ROI_UPPER_LIMIT - 1] + centerLine[CAM_ROI_UPPER_LIMIT - 2]) / 2.0f;
hazheng 80:c85cb93713b3 51
hazheng 80:c85cb93713b3 52 float disY = destY - srcY;
hazheng 80:c85cb93713b3 53 float disX = destX - srcX;
hazheng 80:c85cb93713b3 54
hazheng 80:c85cb93713b3 55 float angleRadians = -1.0f * static_cast<float>(atan(static_cast<double>(disX / disY)));
hazheng 80:c85cb93713b3 56 float angleDegrees = (angleRadians * (180.0f / PI));
hazheng 80:c85cb93713b3 57
hazheng 80:c85cb93713b3 58 ///////////////////////////////////////
hazheng 80:c85cb93713b3 59
hazheng 80:c85cb93713b3 60 /////////////Calculate the offset:
hazheng 80:c85cb93713b3 61 float centerPos = static_cast<float>(RESOLUTION_WIDTH / 2);
hazheng 80:c85cb93713b3 62 float offsetDegrees = ((srcX - centerPos) / centerPos) * SERVO_MAX_ANGLE;
hazheng 80:c85cb93713b3 63 //////////////////////////////////////
hazheng 80:c85cb93713b3 64
hazheng 80:c85cb93713b3 65 const uint8_t lineFoundRefRow = ((2 * CAM_ROI_UPPER_LIMIT) - 1) - (CAM_ROI_UPPER_LIMIT / 2);
hazheng 80:c85cb93713b3 66 //const uint8_t lineFoundRefTop = ((2 * CAM_ROI_UPPER_LIMIT) - 1);
hazheng 80:c85cb93713b3 67 //const uint8_t lineFoundRefBut = (CAM_ROI_UPPER_LIMIT);
hazheng 80:c85cb93713b3 68 const uint8_t isBorderFound = ardu_cam_get_is_border_found();
hazheng 80:c85cb93713b3 69
hazheng 80:c85cb93713b3 70 float totalAngleDegrees = (angleDegrees * 0.52f) + (offsetDegrees * (centerLine[lineFoundRefRow] != BOTH_FOUND ? 0.90f : 0.35f));
hazheng 80:c85cb93713b3 71
hazheng 80:c85cb93713b3 72 if(totalAngleDegrees > SERVO_MAX_ANGLE)
hazheng 80:c85cb93713b3 73 totalAngleDegrees = SERVO_MAX_ANGLE;
hazheng 80:c85cb93713b3 74 else if(totalAngleDegrees < -SERVO_MAX_ANGLE)
hazheng 80:c85cb93713b3 75 totalAngleDegrees = -SERVO_MAX_ANGLE;
hazheng 80:c85cb93713b3 76
hazheng 80:c85cb93713b3 77 if((totalAngleDegrees < 0.0f && (isBorderFound == LEFT_FOUND)) ||
hazheng 80:c85cb93713b3 78 (totalAngleDegrees > 0.0f && (isBorderFound == RIGHT_FOUND)))
hazheng 80:c85cb93713b3 79 {
hazheng 80:c85cb93713b3 80 totalAngleDegrees = ((-1.0f) * totalAngleDegrees);// + ((0.2f) * m_lastAngle);
hazheng 80:c85cb93713b3 81 }
hazheng 80:c85cb93713b3 82 //LOGI("%d %5.3f ", centerLine[lineFoundRefRow], totalAngleDegrees);
hazheng 80:c85cb93713b3 83 servo_set_angle(totalAngleDegrees);
hazheng 80:c85cb93713b3 84
hazheng 80:c85cb93713b3 85
hazheng 80:c85cb93713b3 86 float totalAngleDegreesAbs = abs(totalAngleDegrees);
hazheng 80:c85cb93713b3 87
hazheng 80:c85cb93713b3 88 if(totalAngleDegreesAbs > m_lastAngleAbs)
hazheng 80:c85cb93713b3 89 {
hazheng 80:c85cb93713b3 90 m_cornerRatio = 0.4;//(m_lastAngle / totalAngleDegrees);
hazheng 80:c85cb93713b3 91 }
hazheng 80:c85cb93713b3 92 else if(totalAngleDegreesAbs < m_lastAngleAbs)
hazheng 80:c85cb93713b3 93 {
hazheng 80:c85cb93713b3 94 m_cornerRatio = 0.8;//(m_lastAngle / totalAngleDegrees);
hazheng 80:c85cb93713b3 95 }
hazheng 80:c85cb93713b3 96 else
hazheng 80:c85cb93713b3 97 {
hazheng 80:c85cb93713b3 98 m_cornerRatio = 1.0f;
hazheng 80:c85cb93713b3 99 }
hazheng 80:c85cb93713b3 100
hazheng 80:c85cb93713b3 101 if(totalAngleDegreesAbs > 20.0f)
hazheng 80:c85cb93713b3 102 {
hazheng 80:c85cb93713b3 103 m_speedRatio = 0.350f;
hazheng 80:c85cb93713b3 104 }
hazheng 80:c85cb93713b3 105 else if(totalAngleDegreesAbs > 18.0f)
hazheng 80:c85cb93713b3 106 {
hazheng 80:c85cb93713b3 107 m_speedRatio = 0.80f;
hazheng 80:c85cb93713b3 108 }
hazheng 80:c85cb93713b3 109 else if(totalAngleDegreesAbs > 15.0f)
hazheng 80:c85cb93713b3 110 {
hazheng 80:c85cb93713b3 111 m_speedRatio = 0.90f;
hazheng 80:c85cb93713b3 112 }
hazheng 80:c85cb93713b3 113 else if(totalAngleDegreesAbs > 12.0f)
hazheng 80:c85cb93713b3 114 {
hazheng 80:c85cb93713b3 115 m_speedRatio = 0.92f;
hazheng 80:c85cb93713b3 116 }
hazheng 80:c85cb93713b3 117 else
hazheng 80:c85cb93713b3 118 {
hazheng 80:c85cb93713b3 119 m_speedRatio = 1.0f;
hazheng 80:c85cb93713b3 120 }
hazheng 80:c85cb93713b3 121
hazheng 80:c85cb93713b3 122
hazheng 80:c85cb93713b3 123 if(totalAngleDegrees < 0.0f)
hazheng 80:c85cb93713b3 124 {
hazheng 80:c85cb93713b3 125 motor_set_left_speed(m_speedRatio * m_cornerRatio * ((MOTOR_DIFF_MIN_SPEED) + ((1.0f - MOTOR_DIFF_MIN_SPEED) * ((SERVO_MAX_ANGLE - abs(totalAngleDegrees)) / SERVO_MAX_ANGLE))));
hazheng 80:c85cb93713b3 126 motor_set_right_speed(m_speedRatio * m_cornerRatio * 1.0f);
hazheng 80:c85cb93713b3 127 }
hazheng 80:c85cb93713b3 128 else
hazheng 80:c85cb93713b3 129 {
hazheng 80:c85cb93713b3 130 motor_set_right_speed(m_speedRatio * m_cornerRatio * ((MOTOR_DIFF_MIN_SPEED) + ((1.0f - MOTOR_DIFF_MIN_SPEED) * ((SERVO_MAX_ANGLE - abs(totalAngleDegrees)) / SERVO_MAX_ANGLE))));
hazheng 80:c85cb93713b3 131 motor_set_left_speed(m_speedRatio * m_cornerRatio * 1.0f);
hazheng 80:c85cb93713b3 132 }
hazheng 80:c85cb93713b3 133
hazheng 80:c85cb93713b3 134 m_lastAngle = totalAngleDegrees;
hazheng 80:c85cb93713b3 135 m_lastAngleAbs = totalAngleDegreesAbs;
hazheng 80:c85cb93713b3 136 m_lastLineFound = centerLine[lineFoundRefRow];
hazheng 80:c85cb93713b3 137 }
hazheng 80:c85cb93713b3 138
hazheng 80:c85cb93713b3 139 uint8_t RunningState::HasTouchPosFunction() const
hazheng 80:c85cb93713b3 140 {
hazheng 80:c85cb93713b3 141 return 0;
hazheng 80:c85cb93713b3 142 }
hazheng 80:c85cb93713b3 143
hazheng 80:c85cb93713b3 144 uint8_t RunningState::HasTouchIrqFunction() const
hazheng 80:c85cb93713b3 145 {
hazheng 80:c85cb93713b3 146 return 1;
hazheng 80:c85cb93713b3 147 }
hazheng 80:c85cb93713b3 148
hazheng 80:c85cb93713b3 149 void RunningState::TouchPosCallback(int16_t x, int16_t y)
hazheng 80:c85cb93713b3 150 {
hazheng 80:c85cb93713b3 151
hazheng 80:c85cb93713b3 152 }
hazheng 80:c85cb93713b3 153
hazheng 80:c85cb93713b3 154 void RunningState::TouchIrqCallback()
hazheng 80:c85cb93713b3 155 {
hazheng 80:c85cb93713b3 156 TerminateProcess();
hazheng 80:c85cb93713b3 157 }
hazheng 80:c85cb93713b3 158
hazheng 80:c85cb93713b3 159 void RunningState::TerminateProcess()
hazheng 80:c85cb93713b3 160 {
hazheng 80:c85cb93713b3 161 ardu_utft_print("F", 230, 100);
hazheng 80:c85cb93713b3 162 servo_set_angle(0.0f);
hazheng 80:c85cb93713b3 163 motor_set_right_speed(-1.0f);
hazheng 80:c85cb93713b3 164 motor_set_left_speed(-1.0f);
hazheng 80:c85cb93713b3 165 wait(0.4f);
hazheng 80:c85cb93713b3 166 motor_set_right_speed(0.0f);
hazheng 80:c85cb93713b3 167 motor_set_left_speed(0.0f);
hazheng 80:c85cb93713b3 168 state_manager_clear_current_state();
hazheng 80:c85cb93713b3 169 }