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

Committer:
hazheng
Date:
Thu Apr 06 18:22:02 2017 +0000
Revision:
50:c387c88141fb
Parent:
48:f76b5e252444
Child:
51:a8e58fd3e131
Use smaller image size.

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 float tempCount = 0;
hazheng 18:bf6c5f8281eb 46 //timer.reset();
hazheng 16:66c7a09e71ee 47 timer.start();
hazheng 18:bf6c5f8281eb 48 float timeWas = timer.read();
hazheng 50:c387c88141fb 49 int count = 0;
hazheng 44:15de535c4005 50
hazheng 46:a5eb9bd3bb55 51 motor_set_speeds(0.12f, 0.12f);
hazheng 50:c387c88141fb 52 float speed = 0.12;
hazheng 46:a5eb9bd3bb55 53 servo_set_angle(0.0f);
hazheng 5:577b582e4fe9 54 while (1)
hazheng 5:577b582e4fe9 55 {
hazheng 50:c387c88141fb 56 count = count + 1;
hazheng 50:c387c88141fb 57 if(count % 10000 == 0)
hazheng 50:c387c88141fb 58 {
hazheng 50:c387c88141fb 59 speed = speed + 0.0001f;
hazheng 50:c387c88141fb 60 motor_set_speeds(speed, speed);
hazheng 50:c387c88141fb 61 }
hazheng 18:bf6c5f8281eb 62
hazheng 46:a5eb9bd3bb55 63 //servo_set_angle(0.0f);
hazheng 46:a5eb9bd3bb55 64
hazheng 18:bf6c5f8281eb 65 float deltaTime = timeWas;
hazheng 18:bf6c5f8281eb 66 timeWas = timer.read();
hazheng 18:bf6c5f8281eb 67 deltaTime = timeWas - deltaTime;
hazheng 18:bf6c5f8281eb 68
Bobymicjohn 11:676ea42afd56 69 //led = 0;
hazheng 29:f87d8790f57d 70 g_core.Update(deltaTime);
hazheng 46:a5eb9bd3bb55 71 //motor.Update(deltaTime);
hazheng 45:501b7909139a 72 //servo.Update(deltaTime);
hazheng 25:6f63053cee81 73 //wheelEncoder.Update(deltaTime);
hazheng 29:f87d8790f57d 74 //cam.Update(deltaTime);
hazheng 16:66c7a09e71ee 75
hazheng 18:bf6c5f8281eb 76 tempCount += deltaTime;
hazheng 18:bf6c5f8281eb 77 if(tempCount > 1.0f)
hazheng 5:577b582e4fe9 78 {
hazheng 29:f87d8790f57d 79 //led = !led;
hazheng 18:bf6c5f8281eb 80 tempCount = 0.0f;
hazheng 3:c8867972ffc7 81 }
hazheng 3:c8867972ffc7 82
hazheng 41:7b21c5e3599e 83
hazheng 44:15de535c4005 84 if(!isRegRead && g_core.GetUSBServer().GetStatus() == SER_STAT_RUNNING && timer.read() > 2.5f && ardu_cam_is_capture_finished())
hazheng 13:7dcb1642ef99 85 {
hazheng 32:5badeff825dc 86 //OV7725RegBuf * regBuf = new OV7725RegBuf(g_core);
hazheng 32:5badeff825dc 87 //regBuf->ReadRegisters();
hazheng 32:5badeff825dc 88 //delete regBuf;
hazheng 44:15de535c4005 89 //ardu_cam_print_debug();
hazheng 13:7dcb1642ef99 90 isRegRead = true;
hazheng 13:7dcb1642ef99 91 }
hazheng 41:7b21c5e3599e 92
hazheng 44:15de535c4005 93 //ardu_cam_display_img_utft();
hazheng 46:a5eb9bd3bb55 94
hazheng 44:15de535c4005 95 volatile const uint8_t * centerLine = ardu_cam_get_center_array();
hazheng 46:a5eb9bd3bb55 96
hazheng 46:a5eb9bd3bb55 97 /////////////Calculate the curvature:
hazheng 50:c387c88141fb 98 float srcY = (0.0f + 1.0f + 2.0f) / 3.0f;
hazheng 50:c387c88141fb 99 float srcX = static_cast<float>(centerLine[0] + centerLine[1] + centerLine[2]) / 3.0f;
hazheng 46:a5eb9bd3bb55 100
hazheng 50:c387c88141fb 101 float destY = static_cast<float>(CAM_ROI_UPPER_LIMIT - 1 + (CAM_ROI_UPPER_LIMIT - 2) + (CAM_ROI_UPPER_LIMIT - 3)) / 3.0f;
hazheng 50:c387c88141fb 102 float destX = static_cast<float>(centerLine[CAM_ROI_UPPER_LIMIT - 1] + centerLine[CAM_ROI_UPPER_LIMIT - 2] + centerLine[CAM_ROI_UPPER_LIMIT - 3]) / 3.0f;
hazheng 46:a5eb9bd3bb55 103
hazheng 46:a5eb9bd3bb55 104 float disY = destY - srcY;
hazheng 46:a5eb9bd3bb55 105 float disX = destX - srcX;
hazheng 46:a5eb9bd3bb55 106
hazheng 46:a5eb9bd3bb55 107 float angle = static_cast<float>(atan(static_cast<double>(disX / disY)) * (180.0f / PI));
hazheng 46:a5eb9bd3bb55 108 ///////////////////////////////////////
hazheng 46:a5eb9bd3bb55 109
hazheng 46:a5eb9bd3bb55 110 /////////////Calculate the offset:
hazheng 44:15de535c4005 111 float centerPos = static_cast<float>(RESOLUTION_WIDTH / 2);
hazheng 44:15de535c4005 112 float offsetPercent = (static_cast<float>(centerLine[3]) - centerPos);
hazheng 44:15de535c4005 113 offsetPercent = offsetPercent / centerPos;
hazheng 46:a5eb9bd3bb55 114 //////////////////////////////////////
hazheng 46:a5eb9bd3bb55 115
hazheng 46:a5eb9bd3bb55 116 servo_set_angle((angle * -0.4f) + (offsetPercent * SERVO_MAX_ANGLE));
hazheng 46:a5eb9bd3bb55 117
hazheng 50:c387c88141fb 118
hazheng 50:c387c88141fb 119
hazheng 46:a5eb9bd3bb55 120 //char buf[20];
hazheng 46:a5eb9bd3bb55 121 //sprintf(buf, "angle %f", angle);
hazheng 46:a5eb9bd3bb55 122 //g_core.GetUSBServer().PushUnreliableMsg('D', buf);
hazheng 46:a5eb9bd3bb55 123 /*
hazheng 46:a5eb9bd3bb55 124 std::string tempStr = "XX";
hazheng 46:a5eb9bd3bb55 125 for(uint8_t i = 0; i < CAM_ROI_UPPER_LIMIT; ++i)
hazheng 46:a5eb9bd3bb55 126 {
hazheng 46:a5eb9bd3bb55 127 tempStr[0] = i;
hazheng 46:a5eb9bd3bb55 128 tempStr[1] = centerLine[i];
hazheng 46:a5eb9bd3bb55 129 g_core.GetUSBServer().PushUnreliableMsg('L', tempStr);
hazheng 46:a5eb9bd3bb55 130 }
hazheng 46:a5eb9bd3bb55 131 */
hazheng 46:a5eb9bd3bb55 132
hazheng 46:a5eb9bd3bb55 133 /*
hazheng 44:15de535c4005 134 if(offsetPercent > 0.1)
hazheng 44:15de535c4005 135 {
hazheng 46:a5eb9bd3bb55 136 motor_set_speeds(0.05f, 0.05f);
hazheng 44:15de535c4005 137 }
hazheng 44:15de535c4005 138 else
hazheng 44:15de535c4005 139 {
hazheng 46:a5eb9bd3bb55 140 motor_set_speeds(0.13f, 0.13f);
hazheng 44:15de535c4005 141 }
hazheng 46:a5eb9bd3bb55 142 */
hazheng 43:0d1886f4848a 143 //wait(0.01);
hazheng 3:c8867972ffc7 144 }
hazheng 3:c8867972ffc7 145 }
hazheng 3:c8867972ffc7 146
hazheng 3:c8867972ffc7 147 /*
hazheng 3:c8867972ffc7 148 PwmOut servo(PTE20);
hazheng 3:c8867972ffc7 149
hazheng 3:c8867972ffc7 150 int main() {
hazheng 3:c8867972ffc7 151 servo.period(0.020); // servo requires a 20ms period
hazheng 3:c8867972ffc7 152
hazheng 3:c8867972ffc7 153 while (1) {
hazheng 3:c8867972ffc7 154 for(float offset=0.0; offset<0.001; offset+=0.0001) {
hazheng 3:c8867972ffc7 155 servo.pulsewidth(0.001 + offset); // servo position determined by a pulsewidth between 1-2ms
hazheng 3:c8867972ffc7 156 wait(0.25);
hazheng 3:c8867972ffc7 157 }
hazheng 3:c8867972ffc7 158 }
hazheng 3:c8867972ffc7 159
hazheng 3:c8867972ffc7 160 }
hazheng 13:7dcb1642ef99 161 */
hazheng 13:7dcb1642ef99 162
hazheng 13:7dcb1642ef99 163 /* //code for accelerometer sensor.
hazheng 13:7dcb1642ef99 164 const char regAddr = 0x0D;
hazheng 13:7dcb1642ef99 165 char readValue = 0;
hazheng 13:7dcb1642ef99 166 int result1 = m_sccbCtrl.write(0x1D<<1, &regAddr, 1, true);
hazheng 13:7dcb1642ef99 167 int result2 = m_sccbCtrl.read(0x1D<<1, &readValue, 1, false);
hazheng 13:7dcb1642ef99 168 char buf[20];
hazheng 13:7dcb1642ef99 169 sprintf(buf, "%#x-%#x-%d-%d", regAddr, readValue, result1, result2);
hazheng 13:7dcb1642ef99 170 m_core.GetUSBServer().PushUnreliableMsg('D', buf);
hazheng 3:c8867972ffc7 171 */