main.cpp

00001 #include <mbed.h>
00002 #include "rtos.h"
00003 #include "Periphery/SupportSystem.h"
00004 #include "Misc/SystemTimer.h"
00005 #include "Controller/QuadratureController.h"
00006 #include "Controller/MachineDirectionController.h"
00007 
00008 #define PI 3.14159265
00009 
00010 Serial serialXbee(p28,p27), serialMinnow(p13, p14);
00011 PwmOut drivePWM(p22), steerPWM(p21);
00012 I2C i2c(p9, p10);
00013 
00014 DigitalOut serialLED(LED1), ledTwo(LED2), ledThree(LED3), heartbeatLED(LED4);
00015 
00016 DigitalIn buttonOne(p25), buttonTwo(p26), buttonThree(p29), buttonFour(p30);
00017 
00018 IMU_RegisterDataBuffer_t *IMU_registerDataBuffer;
00019 RadioDecoder_RegisterDataBuffer_t *RadioDecoder_registerDataBuffer;
00020 
00021 QuadratureController *quadratureController;
00022 MachineDirectionController *machineDirectionController;
00023 
00024 // Queues von der Bahnplanung
00025 Queue<float, 2> quadrature_queue;
00026 Queue<float, 2> machine_direction_queue;
00027 float steering_angle_minnow_queue;
00028 float velocity_minnow_queue;
00029 
00030 
00031 // Queues von dem Maussensor
00032 Queue<float, 2> imu_queue_velocity;
00033 Queue<float, 2> imu_queue_steering_angle;
00034 float steering_angle_imu_queue;
00035 float velocity_imu_queue;
00036 
00037 void hearbeat_thread(void const *args);
00038 
00039 void serial_thread(void const *args) {
00040     size_t length = 20;
00041     char receive_buffer[length];
00042     char* ptr = (char*) receive_buffer;
00043     char* end = ptr + length;
00044     char c, x = 22;
00045     float f;
00046     while (true) {
00047         if (ptr != end) {
00048             c = serialMinnow.getc();
00049             *ptr++ = c;
00050         } else {
00051             ptr = (char*)receive_buffer;
00052         }
00053         serialLED = !serialLED;
00054         
00055         if (c == '\r') {
00056             sscanf(receive_buffer, "[%c]=%f\r", &x, &f);
00057             ptr = (char*)receive_buffer;
00058         }
00059         
00060         if (x == 'v') {
00061             machine_direction_queue.put(&f);
00062             
00063             ledTwo = true;
00064             // Buffer wieder auf Anfang setzen
00065             ptr = (char*)receive_buffer;
00066             x = 22;
00067         } else if (x == 'g') {
00068             f = -1*f;
00069             quadrature_queue.put(&f);
00070             ledThree = true;
00071             // Buffer wieder auf Anfang setzen
00072             ptr = (char*)receive_buffer;
00073             x = 22;
00074         }
00075         
00076         //Thread::wait(100);
00077     }
00078 }
00079 
00080 void redirect_machine_direction_controller(void const *args) {
00081     machineDirectionController->cylic_control();
00082 }
00083 
00084 void redirect_quadrature_controller(void const *args) {
00085     quadratureController->cylic_control();
00086 }
00087 
00088 int main() {
00089     serialMinnow.baud(115200);
00090 
00091     drivePWM.period_ms(20);
00092     steerPWM.period_ms(20);
00093 
00094     SystemTimer *millis = new SystemTimer();
00095 
00096     SupportSystem *supportSystem = new SupportSystem(0x80, &i2c);
00097     
00098     quadratureController = new QuadratureController(&steerPWM, &quadrature_queue, &imu_queue_steering_angle);
00099     machineDirectionController = new MachineDirectionController(&drivePWM, &machine_direction_queue, &imu_queue_velocity);
00100 
00101     Thread machineDirectionControl(serial_thread);
00102     Thread hearbeatThread(hearbeat_thread);
00103     
00104     RtosTimer machine_direction_control_timer(redirect_quadrature_controller);
00105     RtosTimer quadrature_control_timer(redirect_quadrature_controller);
00106 
00107     // Konfiguration AMF-IMU
00108     // [0]: Conversation Factor
00109     // [1]: Sensor Position X
00110     // [2]: Sensor Position Y
00111     // [3]: Sensor Position Angle
00112     float configData[4] = {0.002751114f, 167.0f, 0.0f, 271.5f};
00113     
00114     supportSystem->writeImuConfig(configData);
00115     
00116     bool timer_started = false;
00117     
00118     steering_angle_minnow_queue = 0;
00119     quadrature_queue.put(&steering_angle_minnow_queue);
00120     
00121     velocity_minnow_queue = 0;
00122     machine_direction_queue.put(&velocity_minnow_queue);
00123     
00124     machine_direction_control_timer.start(10);
00125     quadrature_control_timer.start(10);
00126 
00127     while(true) {
00128         IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer();
00129         RadioDecoder_registerDataBuffer = supportSystem->getRadioDecoderRegisterDataBuffer();
00130 
00131         uint16_t rc_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[0];
00132         uint8_t rc_valid = RadioDecoder_registerDataBuffer->channelValid[0];
00133         
00134         uint16_t drive_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[1];
00135         uint8_t drive_valid = RadioDecoder_registerDataBuffer->channelValid[1];
00136 
00137         uint16_t steer_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[2];
00138         uint8_t steer_valid = RadioDecoder_registerDataBuffer->channelValid[2];
00139 
00140 
00141         if (rc_percentage > (uint16_t) 1800 && rc_valid != 0) {
00142             // oben => Wettbewerb
00143             supportSystem->setLightManagerRemoteLight(false, true);
00144             if (!timer_started) {
00145                 timer_started = true;
00146                 machine_direction_control_timer.start(10);
00147                 quadrature_control_timer.start(10);
00148             }
00149         } else if (rc_percentage > (uint16_t) 1200 && rc_valid != 0) {
00150             // unten => RC-Wettbewerb
00151             supportSystem->setLightManagerRemoteLight(true, true);
00152             if (drive_valid) {
00153                 drivePWM.pulsewidth_us(drive_percentage);
00154             }
00155             if (steer_valid) {
00156                 steerPWM.pulsewidth_us(steer_percentage);
00157             }
00158             if (timer_started) {
00159                 timer_started = false;
00160                 machine_direction_control_timer.stop();
00161                 quadrature_control_timer.stop();
00162             }
00163         } else if (rc_percentage > (uint16_t) 800 && rc_valid != 0) {
00164             // mitte => RC-Training
00165             supportSystem->setLightManagerRemoteLight(true, true);
00166             if (drive_valid) {
00167                 drivePWM.pulsewidth_us(drive_percentage);
00168             }
00169             if (steer_valid) {
00170                 steerPWM.pulsewidth_us(steer_percentage);
00171             }
00172             if (timer_started) {
00173                 timer_started = false;
00174                 machine_direction_control_timer.stop();
00175                 quadrature_control_timer.stop();
00176             }
00177         }
00178         
00179         velocity_imu_queue = IMU_registerDataBuffer->velocityXFilteredRegister;
00180         imu_queue_velocity.put(&velocity_imu_queue);
00181         
00182         float radius = IMU_registerDataBuffer->velocityXFilteredRegister/IMU_registerDataBuffer->velocityAngularFilteredRegister;
00183         
00184         steering_angle_imu_queue = atan(0.205/radius)*180/PI;
00185         imu_queue_steering_angle.put(&steering_angle_imu_queue);
00186         
00187 
00188         //serialMinnow.printf("%ld, ", difference_millis);
00189         //serialMinnow.printf("%f, ", velocity_minnow_queue);
00190         //serialMinnow.printf("%f, ", steering_angle_minnow_queue);
00191         //serialMinnow.printf("%d, ", IMU_registerDataBuffer->curveRadiusRegister);
00192         //serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityAngularRegister);
00193         //serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityAngularFilteredRegister);
00194         //serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityXRegister);
00195         //serialMinnow.printf("%f\r\n", IMU_registerDataBuffer->velocityXFilteredRegister);
00196         Thread::wait(10);
00197     }
00198 }
00199 
00200 void hearbeat_thread(void const *args) {
00201     while(true) {
00202         heartbeatLED = !heartbeatLED;
00203         Thread::wait(100);
00204     }
00205 }