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@82:992ba6f31e24, 2017-04-18 (annotated)

Committer:
hazheng
Date:
Tue Apr 18 20:51:06 2017 +0000
Branch:
Drift
Revision:
82:992ba6f31e24
Parent:
80:c85cb93713b3
Child:
83:b2c7c6f76575
Finished state switching.

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