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@58:996effac29b9, 2017-04-08 (annotated)

Committer:
Bobymicjohn
Date:
Sat Apr 08 17:44:39 2017 +0000
Revision:
58:996effac29b9
Parent:
57:0d8a155d511d
Child:
61:bdc31e4fa3c4
Added cornering speed factor.

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 57:0d8a155d511d 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"
Bobymicjohn 10:fedb5786a109 18
Bobymicjohn 10:fedb5786a109 19 #include "PinAssignment.h"
Bobymicjohn 8:92f6baeea027 20
hazheng 28:271fc8445e89 21 SPI g_spi_port(PIN_SPI_MOSI, PIN_SPI_MISO, PIN_SPI_SCK);
hazheng 29:f87d8790f57d 22 SW::Core g_core;
hazheng 5:577b582e4fe9 23
hazheng 56:7d3395ae022d 24 #ifdef SW_DEBUG
hazheng 56:7d3395ae022d 25 #include <rtos.h>
hazheng 56:7d3395ae022d 26 Mutex g_sw_spi_lock;
hazheng 56:7d3395ae022d 27 #endif
hazheng 56:7d3395ae022d 28
hazheng 3:c8867972ffc7 29 int main(void) {
hazheng 16:66c7a09e71ee 30
hazheng 16:66c7a09e71ee 31 Timer timer;
hazheng 16:66c7a09e71ee 32
hazheng 48:f76b5e252444 33 g_spi_port.frequency(5000000);
hazheng 29:f87d8790f57d 34 //g_spi_port.format(8, 0);
hazheng 29:f87d8790f57d 35
hazheng 29:f87d8790f57d 36 //SW::Core core;
Bobymicjohn 10:fedb5786a109 37
hazheng 46:a5eb9bd3bb55 38 //Motor motor(g_core);
hazheng 45:501b7909139a 39 //Servo servo(g_core);
hazheng 25:6f63053cee81 40 //WheelEncoder wheelEncoder(core);
hazheng 29:f87d8790f57d 41 //Camera cam(core);
hazheng 46:a5eb9bd3bb55 42 motor_init();
hazheng 45:501b7909139a 43 servo_init();
hazheng 16:66c7a09e71ee 44
hazheng 29:f87d8790f57d 45 bool isRegRead = false;
Bobymicjohn 14:88302da8bff0 46
hazheng 40:be98219930e4 47
hazheng 40:be98219930e4 48 ardu_utft_init();
hazheng 29:f87d8790f57d 49
hazheng 41:7b21c5e3599e 50
hazheng 41:7b21c5e3599e 51 ardu_cam_init();
hazheng 41:7b21c5e3599e 52
hazheng 18:bf6c5f8281eb 53 //timer.reset();
hazheng 16:66c7a09e71ee 54 timer.start();
hazheng 18:bf6c5f8281eb 55 float timeWas = timer.read();
hazheng 44:15de535c4005 56
hazheng 46:a5eb9bd3bb55 57 servo_set_angle(0.0f);
Bobymicjohn 54:f1f5648dfacf 58
Bobymicjohn 54:f1f5648dfacf 59 float speedRatio = 1.0f;
Bobymicjohn 58:996effac29b9 60 float lastAngle = 0.0f;
Bobymicjohn 58:996effac29b9 61 float cornerRatio = 1.0f;
Bobymicjohn 54:f1f5648dfacf 62
hazheng 57:0d8a155d511d 63 DebugCounter counter(10, PTE5);
hazheng 57:0d8a155d511d 64
hazheng 5:577b582e4fe9 65 while (1)
hazheng 5:577b582e4fe9 66 {
hazheng 18:bf6c5f8281eb 67 float deltaTime = timeWas;
hazheng 18:bf6c5f8281eb 68 timeWas = timer.read();
hazheng 18:bf6c5f8281eb 69 deltaTime = timeWas - deltaTime;
hazheng 18:bf6c5f8281eb 70
Bobymicjohn 54:f1f5648dfacf 71 //g_core.Update(deltaTime);
hazheng 41:7b21c5e3599e 72
Bobymicjohn 54:f1f5648dfacf 73 //if(!isRegRead && g_core.GetUSBServer().GetStatus() == SER_STAT_RUNNING && timer.read() > 2.5f && ardu_cam_is_capture_finished())
Bobymicjohn 54:f1f5648dfacf 74 //{
hazheng 44:15de535c4005 75 //ardu_cam_print_debug();
Bobymicjohn 54:f1f5648dfacf 76 // isRegRead = true;
Bobymicjohn 54:f1f5648dfacf 77 //}
hazheng 41:7b21c5e3599e 78
hazheng 44:15de535c4005 79 //ardu_cam_display_img_utft();
hazheng 57:0d8a155d511d 80 image_processing();
hazheng 55:d4db9eef4a9d 81 const volatile uint8_t * centerLine = ardu_cam_get_center_array();
hazheng 46:a5eb9bd3bb55 82
hazheng 46:a5eb9bd3bb55 83 /////////////Calculate the curvature:
hazheng 57:0d8a155d511d 84 float srcY = (0.0f + 1.0f) / 2.0f;
hazheng 57:0d8a155d511d 85 float srcX = static_cast<float>(centerLine[0] + centerLine[1]) / 2.0f;
hazheng 46:a5eb9bd3bb55 86
hazheng 57:0d8a155d511d 87 float destY = static_cast<float>(CAM_ROI_UPPER_LIMIT - 1 + (CAM_ROI_UPPER_LIMIT - 2)) / 2.0f;
hazheng 57:0d8a155d511d 88 float destX = static_cast<float>(centerLine[CAM_ROI_UPPER_LIMIT - 1] + centerLine[CAM_ROI_UPPER_LIMIT - 2]) / 2.0f;
hazheng 46:a5eb9bd3bb55 89
hazheng 46:a5eb9bd3bb55 90 float disY = destY - srcY;
hazheng 46:a5eb9bd3bb55 91 float disX = destX - srcX;
hazheng 46:a5eb9bd3bb55 92
hazheng 52:078b521c9edf 93 float angleRadians = -1.0f * static_cast<float>(atan(static_cast<double>(disX / disY)));
Bobymicjohn 58:996effac29b9 94 float angleDegrees = (angleRadians * (180.0f / PI));
Bobymicjohn 47:a682be9908b9 95
hazheng 46:a5eb9bd3bb55 96 ///////////////////////////////////////
hazheng 46:a5eb9bd3bb55 97
hazheng 46:a5eb9bd3bb55 98 /////////////Calculate the offset:
hazheng 44:15de535c4005 99 float centerPos = static_cast<float>(RESOLUTION_WIDTH / 2);
hazheng 52:078b521c9edf 100 float offsetDegrees = ((srcX - centerPos) / centerPos) * SERVO_MAX_ANGLE;
hazheng 46:a5eb9bd3bb55 101 //////////////////////////////////////
hazheng 46:a5eb9bd3bb55 102
Bobymicjohn 58:996effac29b9 103 float totalAngleDegrees = ((angleDegrees * 0.50f) + (offsetDegrees * 0.35f));
Bobymicjohn 54:f1f5648dfacf 104 float totalAngleDegreesAbs = abs(totalAngleDegrees);
hazheng 52:078b521c9edf 105 servo_set_angle(totalAngleDegrees);
hazheng 52:078b521c9edf 106
hazheng 52:078b521c9edf 107 if(totalAngleDegrees > SERVO_MAX_ANGLE)
hazheng 52:078b521c9edf 108 totalAngleDegrees = SERVO_MAX_ANGLE;
hazheng 52:078b521c9edf 109 else if(totalAngleDegrees < -SERVO_MAX_ANGLE)
hazheng 52:078b521c9edf 110 totalAngleDegrees = -SERVO_MAX_ANGLE;
Bobymicjohn 54:f1f5648dfacf 111
Bobymicjohn 58:996effac29b9 112 if(totalAngleDegrees > lastAngle)
Bobymicjohn 58:996effac29b9 113 {
Bobymicjohn 58:996effac29b9 114 cornerRatio = (lastAngle / totalAngleDegrees);
Bobymicjohn 58:996effac29b9 115 }
Bobymicjohn 58:996effac29b9 116 else if(totalAngleDegrees < lastAngle)
Bobymicjohn 58:996effac29b9 117 {
Bobymicjohn 58:996effac29b9 118 cornerRatio = (lastAngle / totalAngleDegrees);
Bobymicjohn 58:996effac29b9 119 }
Bobymicjohn 58:996effac29b9 120 else
Bobymicjohn 58:996effac29b9 121 {
Bobymicjohn 58:996effac29b9 122 cornerRatio = 1.0f;
Bobymicjohn 58:996effac29b9 123 }
Bobymicjohn 54:f1f5648dfacf 124
Bobymicjohn 58:996effac29b9 125 if(totalAngleDegreesAbs > 18.0f)
hazheng 52:078b521c9edf 126 {
Bobymicjohn 58:996effac29b9 127 speedRatio = 0.90f;
Bobymicjohn 54:f1f5648dfacf 128 }
hazheng 57:0d8a155d511d 129 else if(totalAngleDegreesAbs > 15.0f)
Bobymicjohn 54:f1f5648dfacf 130 {
Bobymicjohn 58:996effac29b9 131 speedRatio = 0.94f;
Bobymicjohn 54:f1f5648dfacf 132 }
Bobymicjohn 58:996effac29b9 133 else if(totalAngleDegreesAbs > 12.0f)
Bobymicjohn 54:f1f5648dfacf 134 {
Bobymicjohn 58:996effac29b9 135 speedRatio = 0.98f;
hazheng 52:078b521c9edf 136 }
hazheng 52:078b521c9edf 137 else
hazheng 52:078b521c9edf 138 {
Bobymicjohn 54:f1f5648dfacf 139 speedRatio = 1.0f;
Bobymicjohn 54:f1f5648dfacf 140 }
Bobymicjohn 54:f1f5648dfacf 141
Bobymicjohn 54:f1f5648dfacf 142 if(totalAngleDegrees < 0.0f)
Bobymicjohn 54:f1f5648dfacf 143 {
Bobymicjohn 58:996effac29b9 144 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 145 motor_set_right_speed(speedRatio * cornerRatio * 1.0f);
Bobymicjohn 54:f1f5648dfacf 146 }
Bobymicjohn 54:f1f5648dfacf 147 else
Bobymicjohn 54:f1f5648dfacf 148 {
Bobymicjohn 58:996effac29b9 149 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 150 motor_set_left_speed(speedRatio * cornerRatio * 1.0f);
hazheng 52:078b521c9edf 151 }
hazheng 56:7d3395ae022d 152
Bobymicjohn 58:996effac29b9 153 lastAngle = totalAngleDegrees;
Bobymicjohn 58:996effac29b9 154
hazheng 57:0d8a155d511d 155 //LOGI("FPS: %f", 1 / deltaTime);
hazheng 57:0d8a155d511d 156 counter.Update();
hazheng 57:0d8a155d511d 157
hazheng 52:078b521c9edf 158 /*
Bobymicjohn 47:a682be9908b9 159 //////// Steer Vehicle / Adjust Speed for Differential //////////
Bobymicjohn 47:a682be9908b9 160 servo_set_angle((angleDegrees * -0.3f) + (offsetPercent * SERVO_MAX_ANGLE * 0.3f));
Bobymicjohn 47:a682be9908b9 161
Bobymicjohn 47:a682be9908b9 162 if(angleRadians > 0.366)
Bobymicjohn 47:a682be9908b9 163 angleRadians = 0.366;
Bobymicjohn 47:a682be9908b9 164 else if(angleRadians < -0.366)
Bobymicjohn 47:a682be9908b9 165 angleRadians = -0.366;
Bobymicjohn 47:a682be9908b9 166
Bobymicjohn 47:a682be9908b9 167 if(angleRadians < 0)
Bobymicjohn 47:a682be9908b9 168 {
Bobymicjohn 47:a682be9908b9 169 motor_set_left_speed((0.5) + (0.5) * ((0.366 - abs(angleRadians))/0.366));
Bobymicjohn 47:a682be9908b9 170 motor_set_right_speed(1.0);
Bobymicjohn 47:a682be9908b9 171 }
Bobymicjohn 47:a682be9908b9 172 else
Bobymicjohn 47:a682be9908b9 173 {
Bobymicjohn 47:a682be9908b9 174 motor_set_right_speed((0.5) + (0.5) * ((0.366 - abs(angleRadians))/0.366));
Bobymicjohn 47:a682be9908b9 175 motor_set_left_speed(1.0);
Bobymicjohn 47:a682be9908b9 176 }
hazheng 52:078b521c9edf 177 */
hazheng 50:c387c88141fb 178
hazheng 50:c387c88141fb 179
hazheng 46:a5eb9bd3bb55 180 //char buf[20];
hazheng 46:a5eb9bd3bb55 181 //sprintf(buf, "angle %f", angle);
hazheng 46:a5eb9bd3bb55 182 //g_core.GetUSBServer().PushUnreliableMsg('D', buf);
hazheng 46:a5eb9bd3bb55 183 /*
hazheng 46:a5eb9bd3bb55 184 std::string tempStr = "XX";
hazheng 46:a5eb9bd3bb55 185 for(uint8_t i = 0; i < CAM_ROI_UPPER_LIMIT; ++i)
hazheng 46:a5eb9bd3bb55 186 {
hazheng 46:a5eb9bd3bb55 187 tempStr[0] = i;
hazheng 46:a5eb9bd3bb55 188 tempStr[1] = centerLine[i];
hazheng 46:a5eb9bd3bb55 189 g_core.GetUSBServer().PushUnreliableMsg('L', tempStr);
hazheng 46:a5eb9bd3bb55 190 }
hazheng 46:a5eb9bd3bb55 191 */
hazheng 46:a5eb9bd3bb55 192
hazheng 46:a5eb9bd3bb55 193 /*
hazheng 44:15de535c4005 194 if(offsetPercent > 0.1)
hazheng 44:15de535c4005 195 {
hazheng 46:a5eb9bd3bb55 196 motor_set_speeds(0.05f, 0.05f);
hazheng 44:15de535c4005 197 }
hazheng 44:15de535c4005 198 else
hazheng 44:15de535c4005 199 {
hazheng 46:a5eb9bd3bb55 200 motor_set_speeds(0.13f, 0.13f);
hazheng 44:15de535c4005 201 }
hazheng 46:a5eb9bd3bb55 202 */
hazheng 43:0d1886f4848a 203 //wait(0.01);
hazheng 3:c8867972ffc7 204 }
hazheng 3:c8867972ffc7 205 }
hazheng 3:c8867972ffc7 206
hazheng 3:c8867972ffc7 207 /*
hazheng 3:c8867972ffc7 208 PwmOut servo(PTE20);
hazheng 3:c8867972ffc7 209
hazheng 3:c8867972ffc7 210 int main() {
hazheng 3:c8867972ffc7 211 servo.period(0.020); // servo requires a 20ms period
hazheng 3:c8867972ffc7 212
hazheng 3:c8867972ffc7 213 while (1) {
hazheng 3:c8867972ffc7 214 for(float offset=0.0; offset<0.001; offset+=0.0001) {
hazheng 3:c8867972ffc7 215 servo.pulsewidth(0.001 + offset); // servo position determined by a pulsewidth between 1-2ms
hazheng 3:c8867972ffc7 216 wait(0.25);
hazheng 3:c8867972ffc7 217 }
hazheng 3:c8867972ffc7 218 }
hazheng 3:c8867972ffc7 219
hazheng 3:c8867972ffc7 220 }
hazheng 13:7dcb1642ef99 221 */
hazheng 13:7dcb1642ef99 222
hazheng 13:7dcb1642ef99 223 /* //code for accelerometer sensor.
hazheng 13:7dcb1642ef99 224 const char regAddr = 0x0D;
hazheng 13:7dcb1642ef99 225 char readValue = 0;
hazheng 13:7dcb1642ef99 226 int result1 = m_sccbCtrl.write(0x1D<<1, &regAddr, 1, true);
hazheng 13:7dcb1642ef99 227 int result2 = m_sccbCtrl.read(0x1D<<1, &readValue, 1, false);
hazheng 13:7dcb1642ef99 228 char buf[20];
hazheng 13:7dcb1642ef99 229 sprintf(buf, "%#x-%#x-%d-%d", regAddr, readValue, result1, result2);
hazheng 13:7dcb1642ef99 230 m_core.GetUSBServer().PushUnreliableMsg('D', buf);
hazheng 3:c8867972ffc7 231 */