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@80:c85cb93713b3, 2017-04-18 (annotated)

Committer:
hazheng
Date:
Tue Apr 18 19:26:33 2017 +0000
Branch:
Drift
Revision:
80:c85cb93713b3
Parent:
79:bdbac82c979b
Child:
82:992ba6f31e24
Transfer to State Machine.

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 80:c85cb93713b3 4
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"
hazheng 80:c85cb93713b3 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 80:c85cb93713b3 18 //#include "IMUManager.h"
hazheng 79:bdbac82c979b 19 #include "ArduTouch.h"
Bobymicjohn 10:fedb5786a109 20
hazheng 80:c85cb93713b3 21 #include "StateManager.h"
hazheng 80:c85cb93713b3 22 #include "RunningState.h"
hazheng 80:c85cb93713b3 23
Bobymicjohn 10:fedb5786a109 24 #include "PinAssignment.h"
Bobymicjohn 8:92f6baeea027 25
hazheng 28:271fc8445e89 26 SPI g_spi_port(PIN_SPI_MOSI, PIN_SPI_MISO, PIN_SPI_SCK);
hazheng 29:f87d8790f57d 27 SW::Core g_core;
hazheng 5:577b582e4fe9 28
hazheng 56:7d3395ae022d 29 #ifdef SW_DEBUG
hazheng 56:7d3395ae022d 30 #include <rtos.h>
hazheng 56:7d3395ae022d 31 Mutex g_sw_spi_lock;
hazheng 56:7d3395ae022d 32 #endif
hazheng 56:7d3395ae022d 33
hazheng 3:c8867972ffc7 34 int main(void) {
hazheng 16:66c7a09e71ee 35
hazheng 16:66c7a09e71ee 36 Timer timer;
hazheng 16:66c7a09e71ee 37
hazheng 48:f76b5e252444 38 g_spi_port.frequency(5000000);
hazheng 29:f87d8790f57d 39 //g_spi_port.format(8, 0);
hazheng 29:f87d8790f57d 40
hazheng 29:f87d8790f57d 41 //SW::Core core;
Bobymicjohn 10:fedb5786a109 42
hazheng 46:a5eb9bd3bb55 43 //Motor motor(g_core);
hazheng 45:501b7909139a 44 //Servo servo(g_core);
hazheng 25:6f63053cee81 45 //WheelEncoder wheelEncoder(core);
hazheng 29:f87d8790f57d 46 //Camera cam(core);
hazheng 46:a5eb9bd3bb55 47 motor_init();
hazheng 45:501b7909139a 48 servo_init();
hazheng 16:66c7a09e71ee 49
hazheng 68:b15cab740371 50 //bool isRegRead = false;
Bobymicjohn 14:88302da8bff0 51
hazheng 40:be98219930e4 52
hazheng 40:be98219930e4 53 ardu_utft_init();
hazheng 29:f87d8790f57d 54
hazheng 41:7b21c5e3599e 55
hazheng 41:7b21c5e3599e 56 ardu_cam_init();
hazheng 41:7b21c5e3599e 57
hazheng 79:bdbac82c979b 58 ardu_touch_init();
hazheng 79:bdbac82c979b 59
hazheng 62:bc5caf59fe39 60
hazheng 64:43ab429a37e0 61 //uint8_t IMUInitResult = imu_manager_init();
hazheng 64:43ab429a37e0 62 //LOGI("IMU Init: %#x", IMUInitResult);
hazheng 64:43ab429a37e0 63 //imu_manager_calibrate();
hazheng 64:43ab429a37e0 64 //imu_manager_begin_tick();
hazheng 62:bc5caf59fe39 65 //wait(0.5);
hazheng 62:bc5caf59fe39 66
hazheng 68:b15cab740371 67 /* DeltaTime Calculation
hazheng 16:66c7a09e71ee 68 timer.start();
hazheng 18:bf6c5f8281eb 69 float timeWas = timer.read();
hazheng 68:b15cab740371 70 */
hazheng 46:a5eb9bd3bb55 71 servo_set_angle(0.0f);
hazheng 80:c85cb93713b3 72
Bobymicjohn 54:f1f5648dfacf 73
hazheng 68:b15cab740371 74 //DebugCounter counter(10, PTE5);
hazheng 57:0d8a155d511d 75
hazheng 80:c85cb93713b3 76
hazheng 80:c85cb93713b3 77 state_manager_set_current_state(new RunningState());
hazheng 80:c85cb93713b3 78
hazheng 5:577b582e4fe9 79 while (1)
hazheng 5:577b582e4fe9 80 {
hazheng 80:c85cb93713b3 81 state_manager_update(0.0f);
hazheng 68:b15cab740371 82 }
hazheng 68:b15cab740371 83 }
hazheng 68:b15cab740371 84
hazheng 68:b15cab740371 85 /*
hazheng 68:b15cab740371 86 PwmOut servo(PTE20);
hazheng 68:b15cab740371 87
hazheng 68:b15cab740371 88 int main() {
hazheng 68:b15cab740371 89 servo.period(0.020); // servo requires a 20ms period
hazheng 68:b15cab740371 90
hazheng 68:b15cab740371 91 while (1) {
hazheng 68:b15cab740371 92 for(float offset=0.0; offset<0.001; offset+=0.0001) {
hazheng 68:b15cab740371 93 servo.pulsewidth(0.001 + offset); // servo position determined by a pulsewidth between 1-2ms
hazheng 68:b15cab740371 94 wait(0.25);
hazheng 68:b15cab740371 95 }
hazheng 68:b15cab740371 96 }
hazheng 68:b15cab740371 97
hazheng 68:b15cab740371 98 }
hazheng 68:b15cab740371 99 */
hazheng 68:b15cab740371 100
hazheng 68:b15cab740371 101 /* //code for accelerometer sensor.
hazheng 68:b15cab740371 102 const char regAddr = 0x0D;
hazheng 68:b15cab740371 103 char readValue = 0;
hazheng 68:b15cab740371 104 int result1 = m_sccbCtrl.write(0x1D<<1, &regAddr, 1, true);
hazheng 68:b15cab740371 105 int result2 = m_sccbCtrl.read(0x1D<<1, &readValue, 1, false);
hazheng 68:b15cab740371 106 char buf[20];
hazheng 68:b15cab740371 107 sprintf(buf, "%#x-%#x-%d-%d", regAddr, readValue, result1, result2);
hazheng 68:b15cab740371 108 m_core.GetUSBServer().PushUnreliableMsg('D', buf);
hazheng 68:b15cab740371 109 */
hazheng 68:b15cab740371 110
hazheng 68:b15cab740371 111 //g_core.Update(deltaTime);
hazheng 68:b15cab740371 112
hazheng 68:b15cab740371 113 //if(!isRegRead && g_core.GetUSBServer().GetStatus() == SER_STAT_RUNNING && timer.read() > 2.5f && ardu_cam_is_capture_finished())
hazheng 68:b15cab740371 114 //{
hazheng 68:b15cab740371 115 //ardu_cam_print_debug();
hazheng 68:b15cab740371 116 // isRegRead = true;
hazheng 68:b15cab740371 117 //}
hazheng 68:b15cab740371 118
hazheng 57:0d8a155d511d 119 //LOGI("FPS: %f", 1 / deltaTime);
hazheng 62:bc5caf59fe39 120
hazheng 62:bc5caf59fe39 121 //imu_manager_init();
hazheng 63:d9a81b3d69f5 122 //imu_manager_update();
hazheng 64:43ab429a37e0 123 //float imuTemp = imu_manager_get_temp();
hazheng 63:d9a81b3d69f5 124
hazheng 63:d9a81b3d69f5 125 //const volatile struct imu_vec3* AccelV = imu_manager_get_accl();
hazheng 63:d9a81b3d69f5 126
hazheng 64:43ab429a37e0 127 //LOGI("A: %5.3f, %5.3f, %5.3f.T%5.2f ", AccelV->x, AccelV->y, AccelV->z, imuTemp);
hazheng 62:bc5caf59fe39 128
hazheng 63:d9a81b3d69f5 129 //const volatile struct imu_vec3* VelocityV = imu_manager_get_velocity();
hazheng 63:d9a81b3d69f5 130
hazheng 64:43ab429a37e0 131 //LOGI("V: %5.3f, %5.3f, %5.3f.T%5.2f ", VelocityV->x, VelocityV->y, VelocityV->z, imuTemp);
hazheng 62:bc5caf59fe39 132
hazheng 64:43ab429a37e0 133 //const volatile struct imu_vec3* PositionV = imu_manager_get_position();
hazheng 63:d9a81b3d69f5 134
hazheng 64:43ab429a37e0 135 //LOGI("P: %5.3f, %5.3f, %5.3f ", PositionV->x, PositionV->y, PositionV->z);
hazheng 62:bc5caf59fe39 136
hazheng 68:b15cab740371 137 //counter.Update();
hazheng 57:0d8a155d511d 138
hazheng 52:078b521c9edf 139 /*
Bobymicjohn 47:a682be9908b9 140 //////// Steer Vehicle / Adjust Speed for Differential //////////
Bobymicjohn 47:a682be9908b9 141 servo_set_angle((angleDegrees * -0.3f) + (offsetPercent * SERVO_MAX_ANGLE * 0.3f));
Bobymicjohn 47:a682be9908b9 142
Bobymicjohn 47:a682be9908b9 143 if(angleRadians > 0.366)
Bobymicjohn 47:a682be9908b9 144 angleRadians = 0.366;
Bobymicjohn 47:a682be9908b9 145 else if(angleRadians < -0.366)
Bobymicjohn 47:a682be9908b9 146 angleRadians = -0.366;
Bobymicjohn 47:a682be9908b9 147
Bobymicjohn 47:a682be9908b9 148 if(angleRadians < 0)
Bobymicjohn 47:a682be9908b9 149 {
Bobymicjohn 47:a682be9908b9 150 motor_set_left_speed((0.5) + (0.5) * ((0.366 - abs(angleRadians))/0.366));
Bobymicjohn 47:a682be9908b9 151 motor_set_right_speed(1.0);
Bobymicjohn 47:a682be9908b9 152 }
Bobymicjohn 47:a682be9908b9 153 else
Bobymicjohn 47:a682be9908b9 154 {
Bobymicjohn 47:a682be9908b9 155 motor_set_right_speed((0.5) + (0.5) * ((0.366 - abs(angleRadians))/0.366));
Bobymicjohn 47:a682be9908b9 156 motor_set_left_speed(1.0);
Bobymicjohn 47:a682be9908b9 157 }
hazheng 52:078b521c9edf 158 */
hazheng 50:c387c88141fb 159
hazheng 50:c387c88141fb 160
hazheng 46:a5eb9bd3bb55 161 //char buf[20];
hazheng 46:a5eb9bd3bb55 162 //sprintf(buf, "angle %f", angle);
hazheng 46:a5eb9bd3bb55 163 //g_core.GetUSBServer().PushUnreliableMsg('D', buf);
hazheng 46:a5eb9bd3bb55 164 /*
hazheng 46:a5eb9bd3bb55 165 std::string tempStr = "XX";
hazheng 46:a5eb9bd3bb55 166 for(uint8_t i = 0; i < CAM_ROI_UPPER_LIMIT; ++i)
hazheng 46:a5eb9bd3bb55 167 {
hazheng 46:a5eb9bd3bb55 168 tempStr[0] = i;
hazheng 46:a5eb9bd3bb55 169 tempStr[1] = centerLine[i];
hazheng 46:a5eb9bd3bb55 170 g_core.GetUSBServer().PushUnreliableMsg('L', tempStr);
hazheng 46:a5eb9bd3bb55 171 }
hazheng 46:a5eb9bd3bb55 172 */
hazheng 46:a5eb9bd3bb55 173
hazheng 46:a5eb9bd3bb55 174 /*
hazheng 44:15de535c4005 175 if(offsetPercent > 0.1)
hazheng 44:15de535c4005 176 {
hazheng 46:a5eb9bd3bb55 177 motor_set_speeds(0.05f, 0.05f);
hazheng 44:15de535c4005 178 }
hazheng 44:15de535c4005 179 else
hazheng 44:15de535c4005 180 {
hazheng 46:a5eb9bd3bb55 181 motor_set_speeds(0.13f, 0.13f);
hazheng 44:15de535c4005 182 }
hazheng 46:a5eb9bd3bb55 183 */
hazheng 68:b15cab740371 184 //wait(0.01);