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@100:ffbeefc9e218, 2017-04-20 (annotated)

Committer:
hazheng
Date:
Thu Apr 20 21:04:10 2017 +0000
Revision:
100:ffbeefc9e218
Parent:
98:fc92bb37ee17 
Better version of Intersection detection.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hazheng 80:c85cb93713b3 1 #include "RunningState.h"
hazheng 80:c85cb93713b3 2
hazheng 100:ffbeefc9e218 3 //#define SW_DEBUG
hazheng 98:fc92bb37ee17 4 #include "SWCommon.h"
hazheng 98:fc92bb37ee17 5
hazheng 80:c85cb93713b3 6 #include "StateManager.h"
hazheng 80:c85cb93713b3 7
hazheng 80:c85cb93713b3 8 #include "ArduCAM.h"
hazheng 80:c85cb93713b3 9 #include "ArduUTFT.h"
hazheng 80:c85cb93713b3 10 #include "Motor.h"
hazheng 80:c85cb93713b3 11 #include "Servo.h"
hazheng 80:c85cb93713b3 12
hazheng 80:c85cb93713b3 13 #define PI 3.14159265f
hazheng 83:b2c7c6f76575 14 #include <math.h>
hazheng 80:c85cb93713b3 15
hazheng 100:ffbeefc9e218 16 //static DigitalOut led_g(LED_GREEN, 1);
hazheng 100:ffbeefc9e218 17
hazheng 80:c85cb93713b3 18 RunningState::RunningState() :
hazheng 80:c85cb93713b3 19 m_speedRatio(1.0f),
hazheng 80:c85cb93713b3 20 m_lastAngle(0.0f),
hazheng 80:c85cb93713b3 21 m_lastAngleAbs(0.0f),
hazheng 80:c85cb93713b3 22 m_cornerRatio(1.0f),
hazheng 86:51048c1f132f 23 //m_lastLineFound(BOTH_FOUND),
hazheng 85:f3911aab41d9 24 m_shouldTerminate(0)
hazheng 80:c85cb93713b3 25 {}
hazheng 80:c85cb93713b3 26
hazheng 80:c85cb93713b3 27 RunningState::~RunningState()
hazheng 80:c85cb93713b3 28 {}
hazheng 80:c85cb93713b3 29
hazheng 80:c85cb93713b3 30 void RunningState::DrawUserInterface()
hazheng 80:c85cb93713b3 31 {
hazheng 81:32bd7a25a699 32 ardu_utft_clr_scr();
hazheng 80:c85cb93713b3 33
hazheng 81:32bd7a25a699 34 ardu_utft_set_color(255, 0, 0);
hazheng 81:32bd7a25a699 35 ardu_utft_fill_rect(10, 10, 310, 230);
hazheng 81:32bd7a25a699 36 ardu_utft_set_color(255, 255, 255);
hazheng 81:32bd7a25a699 37 ardu_utft_print("STOP", 170, 120);
hazheng 80:c85cb93713b3 38 }
hazheng 80:c85cb93713b3 39
hazheng 80:c85cb93713b3 40 void RunningState::Update(float deltaTime)
hazheng 80:c85cb93713b3 41 {
hazheng 80:c85cb93713b3 42 image_processing();
hazheng 80:c85cb93713b3 43 uint8_t shouldTerminate = ardu_cam_get_is_encounter_terminate();
hazheng 80:c85cb93713b3 44
hazheng 85:f3911aab41d9 45 if(shouldTerminate || m_shouldTerminate)
hazheng 80:c85cb93713b3 46 {
hazheng 85:f3911aab41d9 47 m_shouldTerminate = 0;
hazheng 80:c85cb93713b3 48 TerminateProcess();
hazheng 80:c85cb93713b3 49 return;
hazheng 80:c85cb93713b3 50 }
hazheng 80:c85cb93713b3 51
hazheng 80:c85cb93713b3 52 const volatile uint8_t* centerLine = ardu_cam_get_center_array();
hazheng 80:c85cb93713b3 53
hazheng 80:c85cb93713b3 54 /////////////Calculate the curvature:
hazheng 80:c85cb93713b3 55 float srcY = (0.0f + 1.0f) / 2.0f;
hazheng 80:c85cb93713b3 56 float srcX = static_cast<float>(centerLine[0] + centerLine[1]) / 2.0f;
hazheng 80:c85cb93713b3 57
hazheng 80:c85cb93713b3 58 float destY = static_cast<float>(CAM_ROI_UPPER_LIMIT - 1 + (CAM_ROI_UPPER_LIMIT - 2)) / 2.0f;
hazheng 80:c85cb93713b3 59 float destX = static_cast<float>(centerLine[CAM_ROI_UPPER_LIMIT - 1] + centerLine[CAM_ROI_UPPER_LIMIT - 2]) / 2.0f;
hazheng 80:c85cb93713b3 60
hazheng 80:c85cb93713b3 61 float disY = destY - srcY;
hazheng 80:c85cb93713b3 62 float disX = destX - srcX;
hazheng 80:c85cb93713b3 63
hazheng 80:c85cb93713b3 64 float angleRadians = -1.0f * static_cast<float>(atan(static_cast<double>(disX / disY)));
hazheng 80:c85cb93713b3 65 float angleDegrees = (angleRadians * (180.0f / PI));
hazheng 80:c85cb93713b3 66
hazheng 80:c85cb93713b3 67 ///////////////////////////////////////
hazheng 80:c85cb93713b3 68
hazheng 80:c85cb93713b3 69 /////////////Calculate the offset:
hazheng 80:c85cb93713b3 70 float centerPos = static_cast<float>(RESOLUTION_WIDTH / 2);
hazheng 80:c85cb93713b3 71 float offsetDegrees = ((srcX - centerPos) / centerPos) * SERVO_MAX_ANGLE;
hazheng 80:c85cb93713b3 72 //////////////////////////////////////
hazheng 80:c85cb93713b3 73
hazheng 80:c85cb93713b3 74 const uint8_t lineFoundRefRow = ((2 * CAM_ROI_UPPER_LIMIT) - 1) - (CAM_ROI_UPPER_LIMIT / 2);
hazheng 80:c85cb93713b3 75 //const uint8_t lineFoundRefTop = ((2 * CAM_ROI_UPPER_LIMIT) - 1);
hazheng 80:c85cb93713b3 76 //const uint8_t lineFoundRefBut = (CAM_ROI_UPPER_LIMIT);
hazheng 98:fc92bb37ee17 77 //const uint8_t isBorderFound = ardu_cam_get_is_border_found();
hazheng 100:ffbeefc9e218 78 const uint8_t intersectionInfo = ardu_cam_get_intersection_info();
hazheng 80:c85cb93713b3 79
hazheng 100:ffbeefc9e218 80 float totalAngleDegrees = (angleDegrees * 0.55f) + (offsetDegrees * (centerLine[lineFoundRefRow] != BOTH_FOUND ? 0.90f : 0.35f));
hazheng 80:c85cb93713b3 81
hazheng 80:c85cb93713b3 82 if(totalAngleDegrees > SERVO_MAX_ANGLE)
hazheng 80:c85cb93713b3 83 totalAngleDegrees = SERVO_MAX_ANGLE;
hazheng 80:c85cb93713b3 84 else if(totalAngleDegrees < -SERVO_MAX_ANGLE)
hazheng 80:c85cb93713b3 85 totalAngleDegrees = -SERVO_MAX_ANGLE;
hazheng 80:c85cb93713b3 86
hazheng 100:ffbeefc9e218 87 switch(intersectionInfo)
hazheng 100:ffbeefc9e218 88 {
hazheng 100:ffbeefc9e218 89 case INTERSECTION_IN_R:
hazheng 100:ffbeefc9e218 90 totalAngleDegrees = -9.0f;
hazheng 100:ffbeefc9e218 91 //led_g = 0;
hazheng 100:ffbeefc9e218 92 break;
hazheng 100:ffbeefc9e218 93 case INTERSECTION_IN_L:
hazheng 100:ffbeefc9e218 94 totalAngleDegrees = 9.0f;
hazheng 100:ffbeefc9e218 95 //led_g = 0;
hazheng 100:ffbeefc9e218 96 break;
hazheng 100:ffbeefc9e218 97 case INTERSECTION_IN:
hazheng 100:ffbeefc9e218 98 //led_g = 0;
hazheng 100:ffbeefc9e218 99 break;
hazheng 100:ffbeefc9e218 100 default:
hazheng 100:ffbeefc9e218 101 //led_g = 1;
hazheng 100:ffbeefc9e218 102 break;
hazheng 100:ffbeefc9e218 103 }
hazheng 100:ffbeefc9e218 104 /*
hazheng 98:fc92bb37ee17 105 if(isIntersectionDetected)
hazheng 80:c85cb93713b3 106 {
hazheng 82:992ba6f31e24 107 totalAngleDegrees = ((-0.2f) * totalAngleDegrees);// + ((0.2f) * m_lastAngle);
hazheng 80:c85cb93713b3 108 }
hazheng 100:ffbeefc9e218 109 */
hazheng 80:c85cb93713b3 110 //LOGI("%d %5.3f ", centerLine[lineFoundRefRow], totalAngleDegrees);
hazheng 80:c85cb93713b3 111 servo_set_angle(totalAngleDegrees);
hazheng 80:c85cb93713b3 112
hazheng 80:c85cb93713b3 113
hazheng 80:c85cb93713b3 114 float totalAngleDegreesAbs = abs(totalAngleDegrees);
hazheng 80:c85cb93713b3 115
hazheng 80:c85cb93713b3 116 if(totalAngleDegreesAbs > m_lastAngleAbs)
hazheng 80:c85cb93713b3 117 {
hazheng 80:c85cb93713b3 118 m_cornerRatio = 0.4;//(m_lastAngle / totalAngleDegrees);
hazheng 80:c85cb93713b3 119 }
hazheng 80:c85cb93713b3 120 else if(totalAngleDegreesAbs < m_lastAngleAbs)
hazheng 80:c85cb93713b3 121 {
hazheng 80:c85cb93713b3 122 m_cornerRatio = 0.8;//(m_lastAngle / totalAngleDegrees);
hazheng 80:c85cb93713b3 123 }
hazheng 80:c85cb93713b3 124 else
hazheng 80:c85cb93713b3 125 {
hazheng 80:c85cb93713b3 126 m_cornerRatio = 1.0f;
hazheng 80:c85cb93713b3 127 }
hazheng 80:c85cb93713b3 128
hazheng 80:c85cb93713b3 129 if(totalAngleDegreesAbs > 20.0f)
hazheng 80:c85cb93713b3 130 {
hazheng 80:c85cb93713b3 131 m_speedRatio = 0.350f;
hazheng 80:c85cb93713b3 132 }
hazheng 80:c85cb93713b3 133 else if(totalAngleDegreesAbs > 18.0f)
hazheng 80:c85cb93713b3 134 {
hazheng 80:c85cb93713b3 135 m_speedRatio = 0.80f;
hazheng 80:c85cb93713b3 136 }
hazheng 80:c85cb93713b3 137 else if(totalAngleDegreesAbs > 15.0f)
hazheng 80:c85cb93713b3 138 {
hazheng 80:c85cb93713b3 139 m_speedRatio = 0.90f;
hazheng 80:c85cb93713b3 140 }
hazheng 80:c85cb93713b3 141 else if(totalAngleDegreesAbs > 12.0f)
hazheng 80:c85cb93713b3 142 {
hazheng 80:c85cb93713b3 143 m_speedRatio = 0.92f;
hazheng 80:c85cb93713b3 144 }
hazheng 80:c85cb93713b3 145 else
hazheng 80:c85cb93713b3 146 {
hazheng 80:c85cb93713b3 147 m_speedRatio = 1.0f;
hazheng 80:c85cb93713b3 148 }
hazheng 80:c85cb93713b3 149
hazheng 80:c85cb93713b3 150
hazheng 80:c85cb93713b3 151 if(totalAngleDegrees < 0.0f)
hazheng 80:c85cb93713b3 152 {
hazheng 80:c85cb93713b3 153 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 154 motor_set_right_speed(m_speedRatio * m_cornerRatio * 1.0f);
hazheng 80:c85cb93713b3 155 }
hazheng 80:c85cb93713b3 156 else
hazheng 80:c85cb93713b3 157 {
hazheng 80:c85cb93713b3 158 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 159 motor_set_left_speed(m_speedRatio * m_cornerRatio * 1.0f);
hazheng 80:c85cb93713b3 160 }
hazheng 80:c85cb93713b3 161
hazheng 80:c85cb93713b3 162 m_lastAngle = totalAngleDegrees;
hazheng 80:c85cb93713b3 163 m_lastAngleAbs = totalAngleDegreesAbs;
hazheng 86:51048c1f132f 164 //m_lastLineFound = centerLine[lineFoundRefRow];
hazheng 80:c85cb93713b3 165 }
hazheng 80:c85cb93713b3 166
hazheng 80:c85cb93713b3 167 uint8_t RunningState::HasTouchPosFunction() const
hazheng 80:c85cb93713b3 168 {
hazheng 80:c85cb93713b3 169 return 0;
hazheng 80:c85cb93713b3 170 }
hazheng 80:c85cb93713b3 171
hazheng 80:c85cb93713b3 172 uint8_t RunningState::HasTouchIrqFunction() const
hazheng 80:c85cb93713b3 173 {
hazheng 80:c85cb93713b3 174 return 1;
hazheng 80:c85cb93713b3 175 }
hazheng 80:c85cb93713b3 176
hazheng 80:c85cb93713b3 177 void RunningState::TouchIrqCallback()
hazheng 80:c85cb93713b3 178 {
hazheng 85:f3911aab41d9 179 //TerminateProcess();
hazheng 85:f3911aab41d9 180 m_shouldTerminate = 1;
hazheng 80:c85cb93713b3 181 }
hazheng 80:c85cb93713b3 182
hazheng 80:c85cb93713b3 183 void RunningState::TerminateProcess()
hazheng 80:c85cb93713b3 184 {
hazheng 82:992ba6f31e24 185 //ardu_utft_print("F", 230, 100);
hazheng 80:c85cb93713b3 186 servo_set_angle(0.0f);
hazheng 80:c85cb93713b3 187 motor_set_right_speed(-1.0f);
hazheng 80:c85cb93713b3 188 motor_set_left_speed(-1.0f);
hazheng 80:c85cb93713b3 189 wait(0.4f);
hazheng 80:c85cb93713b3 190 motor_set_right_speed(0.0f);
hazheng 80:c85cb93713b3 191 motor_set_left_speed(0.0f);
hazheng 82:992ba6f31e24 192
hazheng 82:992ba6f31e24 193 state_manager_switch_state(STANDBY_STATE);
hazheng 80:c85cb93713b3 194 }