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@55:d4db9eef4a9d, 2017-04-06 (annotated)

Committer:
hazheng
Date:
Thu Apr 06 21:43:15 2017 +0000
Revision:
55:d4db9eef4a9d
Parent:
54:f1f5648dfacf
Child:
56:7d3395ae022d
Fixed a small bug in prev commit

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