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

main.cpp@73:1dcf56e9f1d4, 2017-04-16 (annotated)

Committer:
hazheng
Date:
Sun Apr 16 21:19:45 2017 +0000
Revision:
73:1dcf56e9f1d4
Parent:
71:5601d2faa61d
Child:
74:2d45b954deb1
Fixed stable version 2.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hazheng 3:c8867972ffc7 1 #include "mbed.h"
hazheng 5:577b582e4fe9 2
hazheng 44:15de535c4005 3 #include <math.h>
hazheng 46:a5eb9bd3bb55 4 #define PI 3.14159265f
hazheng 44:15de535c4005 5
hazheng 63:d9a81b3d69f5 6 //#define SW_DEBUG
hazheng 56:7d3395ae022d 7 #include "SWCommon.h"
hazheng 56:7d3395ae022d 8 #include "GlobalVariable.h"
hazheng 56:7d3395ae022d 9
Bobymicjohn 8:92f6baeea027 10 #include "Motor.h"
Bobymicjohn 11:676ea42afd56 11 #include "Servo.h"
Bobymicjohn 15:eb6a274b3dfb 12 #include "WheelEncoder.h"
Bobymicjohn 11:676ea42afd56 13 #include "Core.h"
Bobymicjohn 11:676ea42afd56 14 #include "SWUSBServer.h"
hazheng 29:f87d8790f57d 15
hazheng 29:f87d8790f57d 16 #include "ArduCAM.h"
hazheng 40:be98219930e4 17 #include "ArduUTFT.h"
hazheng 62:bc5caf59fe39 18 #include "IMUManager.h"
Bobymicjohn 10:fedb5786a109 19
Bobymicjohn 10:fedb5786a109 20 #include "PinAssignment.h"
Bobymicjohn 8:92f6baeea027 21
hazheng 28:271fc8445e89 22 SPI g_spi_port(PIN_SPI_MOSI, PIN_SPI_MISO, PIN_SPI_SCK);
hazheng 29:f87d8790f57d 23 SW::Core g_core;
hazheng 5:577b582e4fe9 24
hazheng 56:7d3395ae022d 25 #ifdef SW_DEBUG
hazheng 56:7d3395ae022d 26 #include <rtos.h>
hazheng 56:7d3395ae022d 27 Mutex g_sw_spi_lock;
hazheng 56:7d3395ae022d 28 #endif
hazheng 56:7d3395ae022d 29
hazheng 3:c8867972ffc7 30 int main(void) {
hazheng 16:66c7a09e71ee 31
hazheng 16:66c7a09e71ee 32 Timer timer;
hazheng 16:66c7a09e71ee 33
hazheng 48:f76b5e252444 34 g_spi_port.frequency(5000000);
hazheng 29:f87d8790f57d 35 //g_spi_port.format(8, 0);
hazheng 29:f87d8790f57d 36
hazheng 29:f87d8790f57d 37 //SW::Core core;
Bobymicjohn 10:fedb5786a109 38
hazheng 46:a5eb9bd3bb55 39 //Motor motor(g_core);
hazheng 45:501b7909139a 40 //Servo servo(g_core);
hazheng 25:6f63053cee81 41 //WheelEncoder wheelEncoder(core);
hazheng 29:f87d8790f57d 42 //Camera cam(core);
hazheng 46:a5eb9bd3bb55 43 motor_init();
hazheng 45:501b7909139a 44 servo_init();
hazheng 16:66c7a09e71ee 45
hazheng 68:b15cab740371 46 //bool isRegRead = false;
Bobymicjohn 14:88302da8bff0 47
hazheng 40:be98219930e4 48
hazheng 40:be98219930e4 49 ardu_utft_init();
hazheng 29:f87d8790f57d 50
hazheng 41:7b21c5e3599e 51
hazheng 41:7b21c5e3599e 52 ardu_cam_init();
hazheng 41:7b21c5e3599e 53
hazheng 62:bc5caf59fe39 54
hazheng 64:43ab429a37e0 55 //uint8_t IMUInitResult = imu_manager_init();
hazheng 64:43ab429a37e0 56 //LOGI("IMU Init: %#x", IMUInitResult);
hazheng 64:43ab429a37e0 57 //imu_manager_calibrate();
hazheng 64:43ab429a37e0 58 //imu_manager_begin_tick();
hazheng 62:bc5caf59fe39 59 //wait(0.5);
hazheng 62:bc5caf59fe39 60
hazheng 68:b15cab740371 61 /* DeltaTime Calculation
hazheng 16:66c7a09e71ee 62 timer.start();
hazheng 18:bf6c5f8281eb 63 float timeWas = timer.read();
hazheng 68:b15cab740371 64 */
hazheng 46:a5eb9bd3bb55 65 servo_set_angle(0.0f);
Bobymicjohn 54:f1f5648dfacf 66
Bobymicjohn 54:f1f5648dfacf 67 float speedRatio = 1.0f;
hazheng 73:1dcf56e9f1d4 68 //float lastAngle = 0.0f;
hazheng 71:5601d2faa61d 69 float lastAngleAbs = 0.0f;
Bobymicjohn 58:996effac29b9 70 float cornerRatio = 1.0f;
Bobymicjohn 54:f1f5648dfacf 71
hazheng 68:b15cab740371 72 //DebugCounter counter(10, PTE5);
hazheng 57:0d8a155d511d 73
hazheng 5:577b582e4fe9 74 while (1)
hazheng 5:577b582e4fe9 75 {
hazheng 68:b15cab740371 76 /* DeltaTime Calculation
hazheng 18:bf6c5f8281eb 77 float deltaTime = timeWas;
hazheng 18:bf6c5f8281eb 78 timeWas = timer.read();
hazheng 18:bf6c5f8281eb 79 deltaTime = timeWas - deltaTime;
hazheng 68:b15cab740371 80 */
hazheng 41:7b21c5e3599e 81
hazheng 41:7b21c5e3599e 82
hazheng 44:15de535c4005 83 //ardu_cam_display_img_utft();
hazheng 57:0d8a155d511d 84 image_processing();
hazheng 64:43ab429a37e0 85 uint8_t shouldTerminate = ardu_cam_get_is_encounter_terminate();
hazheng 64:43ab429a37e0 86
hazheng 64:43ab429a37e0 87 if(shouldTerminate)
hazheng 64:43ab429a37e0 88 {
hazheng 65:295c222fdf88 89 ardu_utft_print("F", 230, 100);
hazheng 65:295c222fdf88 90 servo_set_angle(0.0f);
hazheng 66:c9b64f5337cc 91 motor_set_right_speed(-1.0f);
hazheng 66:c9b64f5337cc 92 motor_set_left_speed(-1.0f);
hazheng 68:b15cab740371 93 wait(0.4f);
hazheng 64:43ab429a37e0 94 motor_set_right_speed(0.0f);
hazheng 64:43ab429a37e0 95 motor_set_left_speed(0.0f);
hazheng 64:43ab429a37e0 96 return 0;
hazheng 64:43ab429a37e0 97 }
hazheng 65:295c222fdf88 98 //else
hazheng 65:295c222fdf88 99 //{
hazheng 65:295c222fdf88 100 // ardu_utft_print(" ", 230, 100);
hazheng 65:295c222fdf88 101 //}
hazheng 64:43ab429a37e0 102
hazheng 65:295c222fdf88 103 const volatile uint8_t* centerLine = ardu_cam_get_center_array();
hazheng 46:a5eb9bd3bb55 104
hazheng 46:a5eb9bd3bb55 105 /////////////Calculate the curvature:
hazheng 57:0d8a155d511d 106 float srcY = (0.0f + 1.0f) / 2.0f;
hazheng 57:0d8a155d511d 107 float srcX = static_cast<float>(centerLine[0] + centerLine[1]) / 2.0f;
hazheng 46:a5eb9bd3bb55 108
hazheng 57:0d8a155d511d 109 float destY = static_cast<float>(CAM_ROI_UPPER_LIMIT - 1 + (CAM_ROI_UPPER_LIMIT - 2)) / 2.0f;
hazheng 57:0d8a155d511d 110 float destX = static_cast<float>(centerLine[CAM_ROI_UPPER_LIMIT - 1] + centerLine[CAM_ROI_UPPER_LIMIT - 2]) / 2.0f;
hazheng 46:a5eb9bd3bb55 111
hazheng 46:a5eb9bd3bb55 112 float disY = destY - srcY;
hazheng 46:a5eb9bd3bb55 113 float disX = destX - srcX;
hazheng 46:a5eb9bd3bb55 114
hazheng 52:078b521c9edf 115 float angleRadians = -1.0f * static_cast<float>(atan(static_cast<double>(disX / disY)));
Bobymicjohn 58:996effac29b9 116 float angleDegrees = (angleRadians * (180.0f / PI));
Bobymicjohn 47:a682be9908b9 117
hazheng 46:a5eb9bd3bb55 118 ///////////////////////////////////////
hazheng 46:a5eb9bd3bb55 119
hazheng 46:a5eb9bd3bb55 120 /////////////Calculate the offset:
hazheng 44:15de535c4005 121 float centerPos = static_cast<float>(RESOLUTION_WIDTH / 2);
hazheng 52:078b521c9edf 122 float offsetDegrees = ((srcX - centerPos) / centerPos) * SERVO_MAX_ANGLE;
hazheng 46:a5eb9bd3bb55 123 //////////////////////////////////////
hazheng 46:a5eb9bd3bb55 124
hazheng 73:1dcf56e9f1d4 125 const uint8_t lineFoundRefRow = ((2 * CAM_ROI_UPPER_LIMIT) - 1) - (CAM_ROI_UPPER_LIMIT / 2);
Bobymicjohn 67:1b5c8add3d01 126
hazheng 73:1dcf56e9f1d4 127 float totalAngleDegrees = (angleDegrees * 0.52f) + (offsetDegrees * (centerLine[lineFoundRefRow] != BOTH_FOUND ? 0.90f : 0.35f));
hazheng 73:1dcf56e9f1d4 128
hazheng 73:1dcf56e9f1d4 129 if((totalAngleDegrees < 0.0f && centerLine[lineFoundRefRow] == LEFT_FOUND) ||
hazheng 73:1dcf56e9f1d4 130 (totalAngleDegrees > 0.0f && centerLine[lineFoundRefRow] == RIGHT_FOUND))
hazheng 65:295c222fdf88 131 {
hazheng 71:5601d2faa61d 132 totalAngleDegrees = ((-0.1f) * totalAngleDegrees);// + ((0.2f) * lastAngle);
hazheng 65:295c222fdf88 133 }
hazheng 73:1dcf56e9f1d4 134 LOGI("%d %5.3f ", centerLine[lineFoundRefRow], totalAngleDegrees);
hazheng 52:078b521c9edf 135 servo_set_angle(totalAngleDegrees);
hazheng 52:078b521c9edf 136
hazheng 65:295c222fdf88 137
hazheng 52:078b521c9edf 138 if(totalAngleDegrees > SERVO_MAX_ANGLE)
hazheng 52:078b521c9edf 139 totalAngleDegrees = SERVO_MAX_ANGLE;
hazheng 52:078b521c9edf 140 else if(totalAngleDegrees < -SERVO_MAX_ANGLE)
hazheng 52:078b521c9edf 141 totalAngleDegrees = -SERVO_MAX_ANGLE;
hazheng 73:1dcf56e9f1d4 142
hazheng 73:1dcf56e9f1d4 143 float totalAngleDegreesAbs = abs(totalAngleDegrees);
Bobymicjohn 54:f1f5648dfacf 144
hazheng 71:5601d2faa61d 145 if(totalAngleDegreesAbs > lastAngleAbs)
Bobymicjohn 58:996effac29b9 146 {
hazheng 71:5601d2faa61d 147 cornerRatio = 0.5;//(lastAngle / totalAngleDegrees);
Bobymicjohn 58:996effac29b9 148 }
hazheng 71:5601d2faa61d 149 else if(totalAngleDegreesAbs < lastAngleAbs)
Bobymicjohn 58:996effac29b9 150 {
hazheng 70:311d32a596db 151 cornerRatio = 1.0;//(lastAngle / totalAngleDegrees);
Bobymicjohn 58:996effac29b9 152 }
Bobymicjohn 58:996effac29b9 153 else
Bobymicjohn 58:996effac29b9 154 {
Bobymicjohn 58:996effac29b9 155 cornerRatio = 1.0f;
Bobymicjohn 58:996effac29b9 156 }
Bobymicjohn 54:f1f5648dfacf 157
Bobymicjohn 58:996effac29b9 158 if(totalAngleDegreesAbs > 18.0f)
hazheng 52:078b521c9edf 159 {
Bobymicjohn 58:996effac29b9 160 speedRatio = 0.90f;
Bobymicjohn 54:f1f5648dfacf 161 }
hazheng 57:0d8a155d511d 162 else if(totalAngleDegreesAbs > 15.0f)
Bobymicjohn 54:f1f5648dfacf 163 {
Bobymicjohn 58:996effac29b9 164 speedRatio = 0.94f;
Bobymicjohn 54:f1f5648dfacf 165 }
Bobymicjohn 58:996effac29b9 166 else if(totalAngleDegreesAbs > 12.0f)
Bobymicjohn 54:f1f5648dfacf 167 {
Bobymicjohn 58:996effac29b9 168 speedRatio = 0.98f;
hazheng 52:078b521c9edf 169 }
hazheng 52:078b521c9edf 170 else
hazheng 52:078b521c9edf 171 {
Bobymicjohn 54:f1f5648dfacf 172 speedRatio = 1.0f;
Bobymicjohn 54:f1f5648dfacf 173 }
Bobymicjohn 54:f1f5648dfacf 174
hazheng 65:295c222fdf88 175
Bobymicjohn 54:f1f5648dfacf 176 if(totalAngleDegrees < 0.0f)
Bobymicjohn 54:f1f5648dfacf 177 {
Bobymicjohn 58:996effac29b9 178 motor_set_left_speed(speedRatio * cornerRatio * ((MOTOR_DIFF_MIN_SPEED) + ((1.0f - MOTOR_DIFF_MIN_SPEED) * ((SERVO_MAX_ANGLE - abs(totalAngleDegrees)) / SERVO_MAX_ANGLE))));
Bobymicjohn 58:996effac29b9 179 motor_set_right_speed(speedRatio * cornerRatio * 1.0f);
Bobymicjohn 54:f1f5648dfacf 180 }
Bobymicjohn 54:f1f5648dfacf 181 else
Bobymicjohn 54:f1f5648dfacf 182 {
Bobymicjohn 58:996effac29b9 183 motor_set_right_speed(speedRatio * cornerRatio * ((MOTOR_DIFF_MIN_SPEED) + ((1.0f - MOTOR_DIFF_MIN_SPEED) * ((SERVO_MAX_ANGLE - abs(totalAngleDegrees)) / SERVO_MAX_ANGLE))));
Bobymicjohn 58:996effac29b9 184 motor_set_left_speed(speedRatio * cornerRatio * 1.0f);
hazheng 52:078b521c9edf 185 }
hazheng 65:295c222fdf88 186
hazheng 73:1dcf56e9f1d4 187 //lastAngle = totalAngleDegrees;
hazheng 71:5601d2faa61d 188 lastAngleAbs = totalAngleDegreesAbs;
hazheng 68:b15cab740371 189 }
hazheng 68:b15cab740371 190 }
hazheng 68:b15cab740371 191
hazheng 68:b15cab740371 192 /*
hazheng 68:b15cab740371 193 PwmOut servo(PTE20);
hazheng 68:b15cab740371 194
hazheng 68:b15cab740371 195 int main() {
hazheng 68:b15cab740371 196 servo.period(0.020); // servo requires a 20ms period
hazheng 68:b15cab740371 197
hazheng 68:b15cab740371 198 while (1) {
hazheng 68:b15cab740371 199 for(float offset=0.0; offset<0.001; offset+=0.0001) {
hazheng 68:b15cab740371 200 servo.pulsewidth(0.001 + offset); // servo position determined by a pulsewidth between 1-2ms
hazheng 68:b15cab740371 201 wait(0.25);
hazheng 68:b15cab740371 202 }
hazheng 68:b15cab740371 203 }
hazheng 68:b15cab740371 204
hazheng 68:b15cab740371 205 }
hazheng 68:b15cab740371 206 */
hazheng 68:b15cab740371 207
hazheng 68:b15cab740371 208 /* //code for accelerometer sensor.
hazheng 68:b15cab740371 209 const char regAddr = 0x0D;
hazheng 68:b15cab740371 210 char readValue = 0;
hazheng 68:b15cab740371 211 int result1 = m_sccbCtrl.write(0x1D<<1, &regAddr, 1, true);
hazheng 68:b15cab740371 212 int result2 = m_sccbCtrl.read(0x1D<<1, &readValue, 1, false);
hazheng 68:b15cab740371 213 char buf[20];
hazheng 68:b15cab740371 214 sprintf(buf, "%#x-%#x-%d-%d", regAddr, readValue, result1, result2);
hazheng 68:b15cab740371 215 m_core.GetUSBServer().PushUnreliableMsg('D', buf);
hazheng 68:b15cab740371 216 */
hazheng 68:b15cab740371 217
hazheng 68:b15cab740371 218 //g_core.Update(deltaTime);
hazheng 68:b15cab740371 219
hazheng 68:b15cab740371 220 //if(!isRegRead && g_core.GetUSBServer().GetStatus() == SER_STAT_RUNNING && timer.read() > 2.5f && ardu_cam_is_capture_finished())
hazheng 68:b15cab740371 221 //{
hazheng 68:b15cab740371 222 //ardu_cam_print_debug();
hazheng 68:b15cab740371 223 // isRegRead = true;
hazheng 68:b15cab740371 224 //}
hazheng 68:b15cab740371 225
hazheng 57:0d8a155d511d 226 //LOGI("FPS: %f", 1 / deltaTime);
hazheng 62:bc5caf59fe39 227
hazheng 62:bc5caf59fe39 228 //imu_manager_init();
hazheng 63:d9a81b3d69f5 229 //imu_manager_update();
hazheng 64:43ab429a37e0 230 //float imuTemp = imu_manager_get_temp();
hazheng 63:d9a81b3d69f5 231
hazheng 63:d9a81b3d69f5 232 //const volatile struct imu_vec3* AccelV = imu_manager_get_accl();
hazheng 63:d9a81b3d69f5 233
hazheng 64:43ab429a37e0 234 //LOGI("A: %5.3f, %5.3f, %5.3f.T%5.2f ", AccelV->x, AccelV->y, AccelV->z, imuTemp);
hazheng 62:bc5caf59fe39 235
hazheng 63:d9a81b3d69f5 236 //const volatile struct imu_vec3* VelocityV = imu_manager_get_velocity();
hazheng 63:d9a81b3d69f5 237
hazheng 64:43ab429a37e0 238 //LOGI("V: %5.3f, %5.3f, %5.3f.T%5.2f ", VelocityV->x, VelocityV->y, VelocityV->z, imuTemp);
hazheng 62:bc5caf59fe39 239
hazheng 64:43ab429a37e0 240 //const volatile struct imu_vec3* PositionV = imu_manager_get_position();
hazheng 63:d9a81b3d69f5 241
hazheng 64:43ab429a37e0 242 //LOGI("P: %5.3f, %5.3f, %5.3f ", PositionV->x, PositionV->y, PositionV->z);
hazheng 62:bc5caf59fe39 243
hazheng 68:b15cab740371 244 //counter.Update();
hazheng 57:0d8a155d511d 245
hazheng 52:078b521c9edf 246 /*
Bobymicjohn 47:a682be9908b9 247 //////// Steer Vehicle / Adjust Speed for Differential //////////
Bobymicjohn 47:a682be9908b9 248 servo_set_angle((angleDegrees * -0.3f) + (offsetPercent * SERVO_MAX_ANGLE * 0.3f));
Bobymicjohn 47:a682be9908b9 249
Bobymicjohn 47:a682be9908b9 250 if(angleRadians > 0.366)
Bobymicjohn 47:a682be9908b9 251 angleRadians = 0.366;
Bobymicjohn 47:a682be9908b9 252 else if(angleRadians < -0.366)
Bobymicjohn 47:a682be9908b9 253 angleRadians = -0.366;
Bobymicjohn 47:a682be9908b9 254
Bobymicjohn 47:a682be9908b9 255 if(angleRadians < 0)
Bobymicjohn 47:a682be9908b9 256 {
Bobymicjohn 47:a682be9908b9 257 motor_set_left_speed((0.5) + (0.5) * ((0.366 - abs(angleRadians))/0.366));
Bobymicjohn 47:a682be9908b9 258 motor_set_right_speed(1.0);
Bobymicjohn 47:a682be9908b9 259 }
Bobymicjohn 47:a682be9908b9 260 else
Bobymicjohn 47:a682be9908b9 261 {
Bobymicjohn 47:a682be9908b9 262 motor_set_right_speed((0.5) + (0.5) * ((0.366 - abs(angleRadians))/0.366));
Bobymicjohn 47:a682be9908b9 263 motor_set_left_speed(1.0);
Bobymicjohn 47:a682be9908b9 264 }
hazheng 52:078b521c9edf 265 */
hazheng 50:c387c88141fb 266
hazheng 50:c387c88141fb 267
hazheng 46:a5eb9bd3bb55 268 //char buf[20];
hazheng 46:a5eb9bd3bb55 269 //sprintf(buf, "angle %f", angle);
hazheng 46:a5eb9bd3bb55 270 //g_core.GetUSBServer().PushUnreliableMsg('D', buf);
hazheng 46:a5eb9bd3bb55 271 /*
hazheng 46:a5eb9bd3bb55 272 std::string tempStr = "XX";
hazheng 46:a5eb9bd3bb55 273 for(uint8_t i = 0; i < CAM_ROI_UPPER_LIMIT; ++i)
hazheng 46:a5eb9bd3bb55 274 {
hazheng 46:a5eb9bd3bb55 275 tempStr[0] = i;
hazheng 46:a5eb9bd3bb55 276 tempStr[1] = centerLine[i];
hazheng 46:a5eb9bd3bb55 277 g_core.GetUSBServer().PushUnreliableMsg('L', tempStr);
hazheng 46:a5eb9bd3bb55 278 }
hazheng 46:a5eb9bd3bb55 279 */
hazheng 46:a5eb9bd3bb55 280
hazheng 46:a5eb9bd3bb55 281 /*
hazheng 44:15de535c4005 282 if(offsetPercent > 0.1)
hazheng 44:15de535c4005 283 {
hazheng 46:a5eb9bd3bb55 284 motor_set_speeds(0.05f, 0.05f);
hazheng 44:15de535c4005 285 }
hazheng 44:15de535c4005 286 else
hazheng 44:15de535c4005 287 {
hazheng 46:a5eb9bd3bb55 288 motor_set_speeds(0.13f, 0.13f);
hazheng 44:15de535c4005 289 }
hazheng 46:a5eb9bd3bb55 290 */
hazheng 68:b15cab740371 291 //wait(0.01);