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@64:43ab429a37e0, 2017-04-10 (annotated)

Committer:
hazheng
Date:
Mon Apr 10 16:44:31 2017 +0000
Revision:
64:43ab429a37e0
Parent:
63:d9a81b3d69f5
Child:
65:295c222fdf88
Auto terminate when hit the end of the loop. Commented out all the IMU code, because of the memory space problem.

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 29:f87d8790f57d 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 18:bf6c5f8281eb 61 //timer.reset();
hazheng 16:66c7a09e71ee 62 timer.start();
hazheng 18:bf6c5f8281eb 63 float timeWas = timer.read();
hazheng 44:15de535c4005 64
hazheng 46:a5eb9bd3bb55 65 servo_set_angle(0.0f);
Bobymicjohn 54:f1f5648dfacf 66
Bobymicjohn 54:f1f5648dfacf 67 float speedRatio = 1.0f;
Bobymicjohn 54:f1f5648dfacf 68
hazheng 57:0d8a155d511d 69 DebugCounter counter(10, PTE5);
hazheng 57:0d8a155d511d 70
hazheng 5:577b582e4fe9 71 while (1)
hazheng 5:577b582e4fe9 72 {
hazheng 18:bf6c5f8281eb 73 float deltaTime = timeWas;
hazheng 18:bf6c5f8281eb 74 timeWas = timer.read();
hazheng 18:bf6c5f8281eb 75 deltaTime = timeWas - deltaTime;
hazheng 18:bf6c5f8281eb 76
Bobymicjohn 54:f1f5648dfacf 77 //g_core.Update(deltaTime);
hazheng 41:7b21c5e3599e 78
Bobymicjohn 54:f1f5648dfacf 79 //if(!isRegRead && g_core.GetUSBServer().GetStatus() == SER_STAT_RUNNING && timer.read() > 2.5f && ardu_cam_is_capture_finished())
Bobymicjohn 54:f1f5648dfacf 80 //{
hazheng 44:15de535c4005 81 //ardu_cam_print_debug();
Bobymicjohn 54:f1f5648dfacf 82 // isRegRead = true;
Bobymicjohn 54:f1f5648dfacf 83 //}
hazheng 41:7b21c5e3599e 84
hazheng 44:15de535c4005 85 //ardu_cam_display_img_utft();
hazheng 57:0d8a155d511d 86 image_processing();
hazheng 64:43ab429a37e0 87 uint8_t shouldTerminate = ardu_cam_get_is_encounter_terminate();
hazheng 64:43ab429a37e0 88
hazheng 64:43ab429a37e0 89 if(shouldTerminate)
hazheng 64:43ab429a37e0 90 {
hazheng 64:43ab429a37e0 91 ardu_utft_print("Finished!!!", 230, 100);
hazheng 64:43ab429a37e0 92 motor_set_right_speed(0.0f);
hazheng 64:43ab429a37e0 93 motor_set_left_speed(0.0f);
hazheng 64:43ab429a37e0 94 return 0;
hazheng 64:43ab429a37e0 95 }
hazheng 64:43ab429a37e0 96
hazheng 55:d4db9eef4a9d 97 const volatile uint8_t * centerLine = ardu_cam_get_center_array();
hazheng 46:a5eb9bd3bb55 98
hazheng 46:a5eb9bd3bb55 99 /////////////Calculate the curvature:
hazheng 57:0d8a155d511d 100 float srcY = (0.0f + 1.0f) / 2.0f;
hazheng 57:0d8a155d511d 101 float srcX = static_cast<float>(centerLine[0] + centerLine[1]) / 2.0f;
hazheng 46:a5eb9bd3bb55 102
hazheng 57:0d8a155d511d 103 float destY = static_cast<float>(CAM_ROI_UPPER_LIMIT - 1 + (CAM_ROI_UPPER_LIMIT - 2)) / 2.0f;
hazheng 57:0d8a155d511d 104 float destX = static_cast<float>(centerLine[CAM_ROI_UPPER_LIMIT - 1] + centerLine[CAM_ROI_UPPER_LIMIT - 2]) / 2.0f;
hazheng 46:a5eb9bd3bb55 105
hazheng 46:a5eb9bd3bb55 106 float disY = destY - srcY;
hazheng 46:a5eb9bd3bb55 107 float disX = destX - srcX;
hazheng 46:a5eb9bd3bb55 108
hazheng 52:078b521c9edf 109 float angleRadians = -1.0f * static_cast<float>(atan(static_cast<double>(disX / disY)));
Bobymicjohn 47:a682be9908b9 110
Bobymicjohn 47:a682be9908b9 111 float angleDegrees = (angleRadians * (180.0f / PI));
hazheng 46:a5eb9bd3bb55 112 ///////////////////////////////////////
hazheng 46:a5eb9bd3bb55 113
hazheng 46:a5eb9bd3bb55 114 /////////////Calculate the offset:
hazheng 44:15de535c4005 115 float centerPos = static_cast<float>(RESOLUTION_WIDTH / 2);
hazheng 52:078b521c9edf 116 float offsetDegrees = ((srcX - centerPos) / centerPos) * SERVO_MAX_ANGLE;
hazheng 46:a5eb9bd3bb55 117 //////////////////////////////////////
hazheng 46:a5eb9bd3bb55 118
hazheng 57:0d8a155d511d 119 float totalAngleDegrees = (angleDegrees * 0.50f) + (offsetDegrees * 0.35f);
Bobymicjohn 54:f1f5648dfacf 120 float totalAngleDegreesAbs = abs(totalAngleDegrees);
hazheng 52:078b521c9edf 121 servo_set_angle(totalAngleDegrees);
hazheng 52:078b521c9edf 122
hazheng 52:078b521c9edf 123 if(totalAngleDegrees > SERVO_MAX_ANGLE)
hazheng 52:078b521c9edf 124 totalAngleDegrees = SERVO_MAX_ANGLE;
hazheng 52:078b521c9edf 125 else if(totalAngleDegrees < -SERVO_MAX_ANGLE)
hazheng 52:078b521c9edf 126 totalAngleDegrees = -SERVO_MAX_ANGLE;
Bobymicjohn 54:f1f5648dfacf 127
Bobymicjohn 54:f1f5648dfacf 128
hazheng 57:0d8a155d511d 129 if(totalAngleDegreesAbs > 19.0f)
hazheng 52:078b521c9edf 130 {
hazheng 57:0d8a155d511d 131 speedRatio = 0.82f;
Bobymicjohn 54:f1f5648dfacf 132 }
hazheng 57:0d8a155d511d 133 else if(totalAngleDegreesAbs > 15.0f)
Bobymicjohn 54:f1f5648dfacf 134 {
hazheng 57:0d8a155d511d 135 speedRatio = 0.87f;
Bobymicjohn 54:f1f5648dfacf 136 }
hazheng 57:0d8a155d511d 137 else if(totalAngleDegreesAbs > 10.0f)
Bobymicjohn 54:f1f5648dfacf 138 {
hazheng 57:0d8a155d511d 139 speedRatio = 0.95f;
hazheng 52:078b521c9edf 140 }
hazheng 52:078b521c9edf 141 else
hazheng 52:078b521c9edf 142 {
Bobymicjohn 54:f1f5648dfacf 143 speedRatio = 1.0f;
Bobymicjohn 54:f1f5648dfacf 144 }
Bobymicjohn 54:f1f5648dfacf 145
Bobymicjohn 54:f1f5648dfacf 146 if(totalAngleDegrees < 0.0f)
Bobymicjohn 54:f1f5648dfacf 147 {
Bobymicjohn 54:f1f5648dfacf 148 motor_set_left_speed(speedRatio * ((MOTOR_DIFF_MIN_SPEED) + ((1.0f - MOTOR_DIFF_MIN_SPEED) * ((SERVO_MAX_ANGLE - abs(totalAngleDegrees)) / SERVO_MAX_ANGLE))));
Bobymicjohn 54:f1f5648dfacf 149 motor_set_right_speed(speedRatio * 1.0f);
Bobymicjohn 54:f1f5648dfacf 150 }
Bobymicjohn 54:f1f5648dfacf 151 else
Bobymicjohn 54:f1f5648dfacf 152 {
Bobymicjohn 54:f1f5648dfacf 153 motor_set_right_speed(speedRatio * ((MOTOR_DIFF_MIN_SPEED) + ((1.0f - MOTOR_DIFF_MIN_SPEED) * ((SERVO_MAX_ANGLE - abs(totalAngleDegrees)) / SERVO_MAX_ANGLE))));
Bobymicjohn 54:f1f5648dfacf 154 motor_set_left_speed(speedRatio * 1.0f);
hazheng 52:078b521c9edf 155 }
hazheng 56:7d3395ae022d 156
hazheng 57:0d8a155d511d 157 //LOGI("FPS: %f", 1 / deltaTime);
hazheng 62:bc5caf59fe39 158
hazheng 62:bc5caf59fe39 159 //imu_manager_init();
hazheng 63:d9a81b3d69f5 160 //imu_manager_update();
hazheng 64:43ab429a37e0 161 //float imuTemp = imu_manager_get_temp();
hazheng 63:d9a81b3d69f5 162
hazheng 63:d9a81b3d69f5 163 //const volatile struct imu_vec3* AccelV = imu_manager_get_accl();
hazheng 63:d9a81b3d69f5 164
hazheng 64:43ab429a37e0 165 //LOGI("A: %5.3f, %5.3f, %5.3f.T%5.2f ", AccelV->x, AccelV->y, AccelV->z, imuTemp);
hazheng 62:bc5caf59fe39 166
hazheng 63:d9a81b3d69f5 167 //const volatile struct imu_vec3* VelocityV = imu_manager_get_velocity();
hazheng 63:d9a81b3d69f5 168
hazheng 64:43ab429a37e0 169 //LOGI("V: %5.3f, %5.3f, %5.3f.T%5.2f ", VelocityV->x, VelocityV->y, VelocityV->z, imuTemp);
hazheng 62:bc5caf59fe39 170
hazheng 64:43ab429a37e0 171 //const volatile struct imu_vec3* PositionV = imu_manager_get_position();
hazheng 63:d9a81b3d69f5 172
hazheng 64:43ab429a37e0 173 //LOGI("P: %5.3f, %5.3f, %5.3f ", PositionV->x, PositionV->y, PositionV->z);
hazheng 62:bc5caf59fe39 174
hazheng 57:0d8a155d511d 175 counter.Update();
hazheng 57:0d8a155d511d 176
hazheng 52:078b521c9edf 177 /*
Bobymicjohn 47:a682be9908b9 178 //////// Steer Vehicle / Adjust Speed for Differential //////////
Bobymicjohn 47:a682be9908b9 179 servo_set_angle((angleDegrees * -0.3f) + (offsetPercent * SERVO_MAX_ANGLE * 0.3f));
Bobymicjohn 47:a682be9908b9 180
Bobymicjohn 47:a682be9908b9 181 if(angleRadians > 0.366)
Bobymicjohn 47:a682be9908b9 182 angleRadians = 0.366;
Bobymicjohn 47:a682be9908b9 183 else if(angleRadians < -0.366)
Bobymicjohn 47:a682be9908b9 184 angleRadians = -0.366;
Bobymicjohn 47:a682be9908b9 185
Bobymicjohn 47:a682be9908b9 186 if(angleRadians < 0)
Bobymicjohn 47:a682be9908b9 187 {
Bobymicjohn 47:a682be9908b9 188 motor_set_left_speed((0.5) + (0.5) * ((0.366 - abs(angleRadians))/0.366));
Bobymicjohn 47:a682be9908b9 189 motor_set_right_speed(1.0);
Bobymicjohn 47:a682be9908b9 190 }
Bobymicjohn 47:a682be9908b9 191 else
Bobymicjohn 47:a682be9908b9 192 {
Bobymicjohn 47:a682be9908b9 193 motor_set_right_speed((0.5) + (0.5) * ((0.366 - abs(angleRadians))/0.366));
Bobymicjohn 47:a682be9908b9 194 motor_set_left_speed(1.0);
Bobymicjohn 47:a682be9908b9 195 }
hazheng 52:078b521c9edf 196 */
hazheng 50:c387c88141fb 197
hazheng 50:c387c88141fb 198
hazheng 46:a5eb9bd3bb55 199 //char buf[20];
hazheng 46:a5eb9bd3bb55 200 //sprintf(buf, "angle %f", angle);
hazheng 46:a5eb9bd3bb55 201 //g_core.GetUSBServer().PushUnreliableMsg('D', buf);
hazheng 46:a5eb9bd3bb55 202 /*
hazheng 46:a5eb9bd3bb55 203 std::string tempStr = "XX";
hazheng 46:a5eb9bd3bb55 204 for(uint8_t i = 0; i < CAM_ROI_UPPER_LIMIT; ++i)
hazheng 46:a5eb9bd3bb55 205 {
hazheng 46:a5eb9bd3bb55 206 tempStr[0] = i;
hazheng 46:a5eb9bd3bb55 207 tempStr[1] = centerLine[i];
hazheng 46:a5eb9bd3bb55 208 g_core.GetUSBServer().PushUnreliableMsg('L', tempStr);
hazheng 46:a5eb9bd3bb55 209 }
hazheng 46:a5eb9bd3bb55 210 */
hazheng 46:a5eb9bd3bb55 211
hazheng 46:a5eb9bd3bb55 212 /*
hazheng 44:15de535c4005 213 if(offsetPercent > 0.1)
hazheng 44:15de535c4005 214 {
hazheng 46:a5eb9bd3bb55 215 motor_set_speeds(0.05f, 0.05f);
hazheng 44:15de535c4005 216 }
hazheng 44:15de535c4005 217 else
hazheng 44:15de535c4005 218 {
hazheng 46:a5eb9bd3bb55 219 motor_set_speeds(0.13f, 0.13f);
hazheng 44:15de535c4005 220 }
hazheng 46:a5eb9bd3bb55 221 */
hazheng 43:0d1886f4848a 222 //wait(0.01);
hazheng 3:c8867972ffc7 223 }
hazheng 3:c8867972ffc7 224 }
hazheng 3:c8867972ffc7 225
hazheng 3:c8867972ffc7 226 /*
hazheng 3:c8867972ffc7 227 PwmOut servo(PTE20);
hazheng 3:c8867972ffc7 228
hazheng 3:c8867972ffc7 229 int main() {
hazheng 3:c8867972ffc7 230 servo.period(0.020); // servo requires a 20ms period
hazheng 3:c8867972ffc7 231
hazheng 3:c8867972ffc7 232 while (1) {
hazheng 3:c8867972ffc7 233 for(float offset=0.0; offset<0.001; offset+=0.0001) {
hazheng 3:c8867972ffc7 234 servo.pulsewidth(0.001 + offset); // servo position determined by a pulsewidth between 1-2ms
hazheng 3:c8867972ffc7 235 wait(0.25);
hazheng 3:c8867972ffc7 236 }
hazheng 3:c8867972ffc7 237 }
hazheng 3:c8867972ffc7 238
hazheng 3:c8867972ffc7 239 }
hazheng 13:7dcb1642ef99 240 */
hazheng 13:7dcb1642ef99 241
hazheng 13:7dcb1642ef99 242 /* //code for accelerometer sensor.
hazheng 13:7dcb1642ef99 243 const char regAddr = 0x0D;
hazheng 13:7dcb1642ef99 244 char readValue = 0;
hazheng 13:7dcb1642ef99 245 int result1 = m_sccbCtrl.write(0x1D<<1, &regAddr, 1, true);
hazheng 13:7dcb1642ef99 246 int result2 = m_sccbCtrl.read(0x1D<<1, &readValue, 1, false);
hazheng 13:7dcb1642ef99 247 char buf[20];
hazheng 13:7dcb1642ef99 248 sprintf(buf, "%#x-%#x-%d-%d", regAddr, readValue, result1, result2);
hazheng 13:7dcb1642ef99 249 m_core.GetUSBServer().PushUnreliableMsg('D', buf);
hazheng 3:c8867972ffc7 250 */