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@44:15de535c4005, 2017-03-29 (annotated)

Committer:
hazheng
Date:
Wed Mar 29 21:19:27 2017 +0000
Revision:
44:15de535c4005
Parent:
43:0d1886f4848a
Child:
45:501b7909139a
Made the car move!...

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 44:15de535c4005 4
Bobymicjohn 8:92f6baeea027 5 #include "Motor.h"
Bobymicjohn 11:676ea42afd56 6 #include "Servo.h"
Bobymicjohn 15:eb6a274b3dfb 7 #include "WheelEncoder.h"
Bobymicjohn 11:676ea42afd56 8 #include "Core.h"
Bobymicjohn 11:676ea42afd56 9 #include "SWUSBServer.h"
hazheng 29:f87d8790f57d 10
hazheng 29:f87d8790f57d 11 #include "ArduCAM.h"
hazheng 40:be98219930e4 12 #include "ArduUTFT.h"
Bobymicjohn 10:fedb5786a109 13
Bobymicjohn 10:fedb5786a109 14 #include "PinAssignment.h"
hazheng 29:f87d8790f57d 15 #include "GlobalVariable.h"
Bobymicjohn 8:92f6baeea027 16
hazheng 28:271fc8445e89 17 SPI g_spi_port(PIN_SPI_MOSI, PIN_SPI_MISO, PIN_SPI_SCK);
hazheng 29:f87d8790f57d 18 SW::Core g_core;
hazheng 5:577b582e4fe9 19
hazheng 3:c8867972ffc7 20 int main(void) {
hazheng 16:66c7a09e71ee 21
hazheng 16:66c7a09e71ee 22 Timer timer;
hazheng 16:66c7a09e71ee 23
hazheng 44:15de535c4005 24 g_spi_port.frequency(5500000);
hazheng 29:f87d8790f57d 25 //g_spi_port.format(8, 0);
hazheng 29:f87d8790f57d 26
hazheng 29:f87d8790f57d 27 //SW::Core core;
Bobymicjohn 10:fedb5786a109 28
hazheng 44:15de535c4005 29 Motor motor(g_core);
hazheng 44:15de535c4005 30 Servo servo(g_core);
hazheng 25:6f63053cee81 31 //WheelEncoder wheelEncoder(core);
hazheng 29:f87d8790f57d 32 //Camera cam(core);
hazheng 6:0682e1c90119 33
hazheng 16:66c7a09e71ee 34
hazheng 29:f87d8790f57d 35 bool isRegRead = false;
Bobymicjohn 14:88302da8bff0 36
hazheng 40:be98219930e4 37
hazheng 40:be98219930e4 38 ardu_utft_init();
hazheng 29:f87d8790f57d 39
hazheng 41:7b21c5e3599e 40
hazheng 41:7b21c5e3599e 41 ardu_cam_init();
hazheng 41:7b21c5e3599e 42
hazheng 18:bf6c5f8281eb 43 float tempCount = 0;
hazheng 18:bf6c5f8281eb 44 //timer.reset();
hazheng 16:66c7a09e71ee 45 timer.start();
hazheng 18:bf6c5f8281eb 46 float timeWas = timer.read();
hazheng 44:15de535c4005 47
hazheng 44:15de535c4005 48 motor.setLeftSpeed(0.12f);
hazheng 44:15de535c4005 49 motor.setRightSpeed(0.12f);
hazheng 5:577b582e4fe9 50 while (1)
hazheng 5:577b582e4fe9 51 {
hazheng 18:bf6c5f8281eb 52
hazheng 18:bf6c5f8281eb 53 float deltaTime = timeWas;
hazheng 18:bf6c5f8281eb 54 timeWas = timer.read();
hazheng 18:bf6c5f8281eb 55 deltaTime = timeWas - deltaTime;
hazheng 18:bf6c5f8281eb 56
Bobymicjohn 11:676ea42afd56 57 //led = 0;
hazheng 29:f87d8790f57d 58 g_core.Update(deltaTime);
hazheng 44:15de535c4005 59 motor.Update(deltaTime);
hazheng 44:15de535c4005 60 servo.Update(deltaTime);
hazheng 25:6f63053cee81 61 //wheelEncoder.Update(deltaTime);
hazheng 29:f87d8790f57d 62 //cam.Update(deltaTime);
hazheng 16:66c7a09e71ee 63
hazheng 18:bf6c5f8281eb 64 tempCount += deltaTime;
hazheng 18:bf6c5f8281eb 65 if(tempCount > 1.0f)
hazheng 5:577b582e4fe9 66 {
hazheng 29:f87d8790f57d 67 //led = !led;
hazheng 18:bf6c5f8281eb 68 tempCount = 0.0f;
hazheng 3:c8867972ffc7 69 }
hazheng 3:c8867972ffc7 70
hazheng 41:7b21c5e3599e 71
hazheng 44:15de535c4005 72 if(!isRegRead && g_core.GetUSBServer().GetStatus() == SER_STAT_RUNNING && timer.read() > 2.5f && ardu_cam_is_capture_finished())
hazheng 13:7dcb1642ef99 73 {
hazheng 32:5badeff825dc 74 //OV7725RegBuf * regBuf = new OV7725RegBuf(g_core);
hazheng 32:5badeff825dc 75 //regBuf->ReadRegisters();
hazheng 32:5badeff825dc 76 //delete regBuf;
hazheng 44:15de535c4005 77 //ardu_cam_print_debug();
hazheng 13:7dcb1642ef99 78 isRegRead = true;
hazheng 13:7dcb1642ef99 79 }
hazheng 41:7b21c5e3599e 80
hazheng 44:15de535c4005 81 //ardu_cam_display_img_utft();
hazheng 44:15de535c4005 82 volatile const uint8_t * centerLine = ardu_cam_get_center_array();
hazheng 44:15de535c4005 83 float centerPos = static_cast<float>(RESOLUTION_WIDTH / 2);
hazheng 44:15de535c4005 84 float offsetPercent = (static_cast<float>(centerLine[3]) - centerPos);
hazheng 44:15de535c4005 85 offsetPercent = offsetPercent / centerPos;
hazheng 44:15de535c4005 86 servo.setAngle(offsetPercent * 1.7f * SERVO_MAX_ANGLE);
hazheng 44:15de535c4005 87 if(offsetPercent > 0.1)
hazheng 44:15de535c4005 88 {
hazheng 44:15de535c4005 89 motor.setLeftSpeed(0.05f);
hazheng 44:15de535c4005 90 motor.setRightSpeed(0.05f);
hazheng 44:15de535c4005 91 }
hazheng 44:15de535c4005 92 else
hazheng 44:15de535c4005 93 {
hazheng 44:15de535c4005 94 motor.setLeftSpeed(0.13f);
hazheng 44:15de535c4005 95 motor.setRightSpeed(0.13f);
hazheng 44:15de535c4005 96 }
hazheng 29:f87d8790f57d 97
hazheng 43:0d1886f4848a 98 //wait(0.01);
hazheng 3:c8867972ffc7 99 }
hazheng 3:c8867972ffc7 100 }
hazheng 3:c8867972ffc7 101
hazheng 3:c8867972ffc7 102 /*
hazheng 3:c8867972ffc7 103 PwmOut servo(PTE20);
hazheng 3:c8867972ffc7 104
hazheng 3:c8867972ffc7 105 int main() {
hazheng 3:c8867972ffc7 106 servo.period(0.020); // servo requires a 20ms period
hazheng 3:c8867972ffc7 107
hazheng 3:c8867972ffc7 108 while (1) {
hazheng 3:c8867972ffc7 109 for(float offset=0.0; offset<0.001; offset+=0.0001) {
hazheng 3:c8867972ffc7 110 servo.pulsewidth(0.001 + offset); // servo position determined by a pulsewidth between 1-2ms
hazheng 3:c8867972ffc7 111 wait(0.25);
hazheng 3:c8867972ffc7 112 }
hazheng 3:c8867972ffc7 113 }
hazheng 3:c8867972ffc7 114
hazheng 3:c8867972ffc7 115 }
hazheng 13:7dcb1642ef99 116 */
hazheng 13:7dcb1642ef99 117
hazheng 13:7dcb1642ef99 118 /* //code for accelerometer sensor.
hazheng 13:7dcb1642ef99 119 const char regAddr = 0x0D;
hazheng 13:7dcb1642ef99 120 char readValue = 0;
hazheng 13:7dcb1642ef99 121 int result1 = m_sccbCtrl.write(0x1D<<1, &regAddr, 1, true);
hazheng 13:7dcb1642ef99 122 int result2 = m_sccbCtrl.read(0x1D<<1, &readValue, 1, false);
hazheng 13:7dcb1642ef99 123 char buf[20];
hazheng 13:7dcb1642ef99 124 sprintf(buf, "%#x-%#x-%d-%d", regAddr, readValue, result1, result2);
hazheng 13:7dcb1642ef99 125 m_core.GetUSBServer().PushUnreliableMsg('D', buf);
hazheng 3:c8867972ffc7 126 */