Matti Borchers / Mbed 2 deprecated mbed_amf_controlsystem_iO_copy

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-rtos mbed

Fork of mbed_amf_controlsystem_iO_copy by Oliver Wenzel

main.cpp@15:141de2b5646d, 2016-02-07 (annotated)

Committer:
mborchers
Date:
Sun Feb 07 18:55:50 2016 +0000
Revision:
15:141de2b5646d
Parent:
14:48cdd880ca1a
Child:
16:a387f2e275c8
xfd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mborchers 0:8a6003b8bb5b 1 #include <mbed.h>
mborchers 0:8a6003b8bb5b 2 #include "rtos.h"
mborchers 0:8a6003b8bb5b 3 #include "Periphery/SupportSystem.h"
mborchers 0:8a6003b8bb5b 4 #include "Misc/SystemTimer.h"
mborchers 3:391c4639bc7d 5 #include "Controller/QuadratureController.h"
mborchers 13:34f7f783ad24 6 #include "Controller/MachineDirectionController.h"
mborchers 0:8a6003b8bb5b 7
mborchers 0:8a6003b8bb5b 8 #define PI 3.14159265
mborchers 0:8a6003b8bb5b 9
mborchers 13:34f7f783ad24 10 Serial serialXbee(p28,p27), serialMinnow(p13, p14);
mborchers 13:34f7f783ad24 11 PwmOut drivePWM(p22), steerPWM(p21);
mborchers 0:8a6003b8bb5b 12 I2C i2c(p9, p10);
mborchers 0:8a6003b8bb5b 13
mborchers 13:34f7f783ad24 14 DigitalOut serialLED(LED1), ledTwo(LED2), ledThree(LED3), heartbeatLED(LED4);
mborchers 0:8a6003b8bb5b 15
mborchers 13:34f7f783ad24 16 DigitalIn buttonOne(p25), buttonTwo(p26), buttonThree(p29), buttonFour(p30);
mborchers 0:8a6003b8bb5b 17
mborchers 0:8a6003b8bb5b 18 IMU_RegisterDataBuffer_t *IMU_registerDataBuffer;
mborchers 0:8a6003b8bb5b 19 RadioDecoder_RegisterDataBuffer_t *RadioDecoder_registerDataBuffer;
mborchers 0:8a6003b8bb5b 20
mborchers 4:f0be27a5a83a 21 QuadratureController *quadratureController;
mborchers 13:34f7f783ad24 22 MachineDirectionController *machineDirectionController;
mborchers 4:f0be27a5a83a 23
mborchers 0:8a6003b8bb5b 24 // Queues von der Bahnplanung
mborchers 0:8a6003b8bb5b 25 Queue<float, 2> quadrature_queue;
mborchers 0:8a6003b8bb5b 26 Queue<float, 2> machine_direction_queue;
mborchers 0:8a6003b8bb5b 27 float steering_angle_minnow_queue;
mborchers 0:8a6003b8bb5b 28 float velocity_minnow_queue;
mborchers 0:8a6003b8bb5b 29
mborchers 0:8a6003b8bb5b 30
mborchers 0:8a6003b8bb5b 31 // Queues von dem Maussensor
mborchers 0:8a6003b8bb5b 32 Queue<float, 2> imu_queue_velocity;
mborchers 0:8a6003b8bb5b 33 Queue<float, 2> imu_queue_steering_angle;
mborchers 0:8a6003b8bb5b 34 float steering_angle_imu_queue;
mborchers 0:8a6003b8bb5b 35 float velocity_imu_queue;
mborchers 0:8a6003b8bb5b 36
mborchers 15:141de2b5646d 37 // Queue
mborchers 15:141de2b5646d 38 typedef struct {
mborchers 15:141de2b5646d 39 char identifier;
mborchers 15:141de2b5646d 40 uint8_t payload;
mborchers 15:141de2b5646d 41 } serial_output_t;
mborchers 15:141de2b5646d 42 serial_output_t modus_struct;
mborchers 15:141de2b5646d 43 Queue<serial_output_t, 2> serial_output_modus_queue;
mborchers 15:141de2b5646d 44
mborchers 10:f92664a22d02 45 void hearbeat_thread(void const *args);
mborchers 10:f92664a22d02 46
mborchers 15:141de2b5646d 47 void serial_transmit_thread(void const *args) {
mborchers 15:141de2b5646d 48 //serialMinnow.printf("Serial Thread erstellt");
mborchers 15:141de2b5646d 49 osEvent modus_event;
mborchers 15:141de2b5646d 50 while (true) {
mborchers 15:141de2b5646d 51 //modus_event = serial_output_modus_queue.get();
mborchers 15:141de2b5646d 52 if (modus_event.status == osEventMessage) {
mborchers 15:141de2b5646d 53 serialMinnow.printf("in der if\r\n");
mborchers 15:141de2b5646d 54 serialMinnow.printf("[%c]=%d\r", ((serial_output_t *)modus_event.value.p)->identifier, ((serial_output_t *)modus_event.value.p)->payload);
mborchers 15:141de2b5646d 55 }
mborchers 15:141de2b5646d 56 }
mborchers 15:141de2b5646d 57 }
mborchers 15:141de2b5646d 58
mborchers 15:141de2b5646d 59 void serial_receive_thread(void const *args) {
mborchers 10:f92664a22d02 60 size_t length = 20;
mborchers 10:f92664a22d02 61 char receive_buffer[length];
mborchers 10:f92664a22d02 62 char* ptr = (char*) receive_buffer;
mborchers 10:f92664a22d02 63 char* end = ptr + length;
mborchers 10:f92664a22d02 64 char c, x = 22;
mborchers 7:c5940ae7773a 65 float f;
mborchers 0:8a6003b8bb5b 66 while (true) {
mborchers 10:f92664a22d02 67 if (ptr != end) {
mborchers 10:f92664a22d02 68 c = serialMinnow.getc();
mborchers 10:f92664a22d02 69 *ptr++ = c;
mborchers 10:f92664a22d02 70 } else {
mborchers 10:f92664a22d02 71 ptr = (char*)receive_buffer;
mborchers 7:c5940ae7773a 72 }
mborchers 13:34f7f783ad24 73 serialLED = !serialLED;
mborchers 10:f92664a22d02 74
mborchers 12:221c9e02ea96 75 if (c == '\r') {
mborchers 13:34f7f783ad24 76 sscanf(receive_buffer, "[%c]=%f\r", &x, &f);
mborchers 12:221c9e02ea96 77 ptr = (char*)receive_buffer;
mborchers 12:221c9e02ea96 78 }
mborchers 10:f92664a22d02 79
mborchers 10:f92664a22d02 80 if (x == 'v') {
mborchers 12:221c9e02ea96 81 machine_direction_queue.put(&f);
mborchers 12:221c9e02ea96 82
mborchers 13:34f7f783ad24 83 ledTwo = true;
mborchers 10:f92664a22d02 84 // Buffer wieder auf Anfang setzen
mborchers 10:f92664a22d02 85 ptr = (char*)receive_buffer;
mborchers 10:f92664a22d02 86 x = 22;
mborchers 10:f92664a22d02 87 } else if (x == 'g') {
mborchers 13:34f7f783ad24 88 f = -1*f;
mborchers 12:221c9e02ea96 89 quadrature_queue.put(&f);
mborchers 13:34f7f783ad24 90 ledThree = true;
mborchers 10:f92664a22d02 91 // Buffer wieder auf Anfang setzen
mborchers 10:f92664a22d02 92 ptr = (char*)receive_buffer;
mborchers 10:f92664a22d02 93 x = 22;
mborchers 10:f92664a22d02 94 }
mborchers 15:141de2b5646d 95 //if(rc_valid != 0) {
mborchers 15:141de2b5646d 96 // serialMinnow.putc('a');//Aktiviert
mborchers 15:141de2b5646d 97 //} else {
mborchers 15:141de2b5646d 98 // serialMinnow.putc('d');//Deaktiviert
mborchers 15:141de2b5646d 99 //}
mborchers 10:f92664a22d02 100
mborchers 7:c5940ae7773a 101 //Thread::wait(100);
mborchers 0:8a6003b8bb5b 102 }
mborchers 0:8a6003b8bb5b 103 }
mborchers 0:8a6003b8bb5b 104
mborchers 13:34f7f783ad24 105 void redirect_machine_direction_controller(void const *args) {
mborchers 13:34f7f783ad24 106 machineDirectionController->cylic_control();
mborchers 0:8a6003b8bb5b 107 }
mborchers 0:8a6003b8bb5b 108
mborchers 4:f0be27a5a83a 109 void redirect_quadrature_controller(void const *args) {
mborchers 4:f0be27a5a83a 110 quadratureController->cylic_control();
mborchers 4:f0be27a5a83a 111 }
mborchers 4:f0be27a5a83a 112
mborchers 0:8a6003b8bb5b 113 int main() {
mborchers 0:8a6003b8bb5b 114 serialMinnow.baud(115200);
mborchers 0:8a6003b8bb5b 115
mborchers 0:8a6003b8bb5b 116 drivePWM.period_ms(20);
mborchers 0:8a6003b8bb5b 117 steerPWM.period_ms(20);
mborchers 0:8a6003b8bb5b 118
mborchers 0:8a6003b8bb5b 119 SystemTimer *millis = new SystemTimer();
mborchers 0:8a6003b8bb5b 120
mborchers 0:8a6003b8bb5b 121 SupportSystem *supportSystem = new SupportSystem(0x80, &i2c);
mborchers 3:391c4639bc7d 122
torstenwylegala 6:aa27bc8c58f5 123 quadratureController = new QuadratureController(&steerPWM, &quadrature_queue, &imu_queue_steering_angle);
mborchers 13:34f7f783ad24 124 machineDirectionController = new MachineDirectionController(&drivePWM, &machine_direction_queue, &imu_queue_velocity);
mborchers 0:8a6003b8bb5b 125
mborchers 15:141de2b5646d 126 //Thread serialTransmitThread(serial_transmit_thread);
mborchers 15:141de2b5646d 127 Thread machineDirectionControl(serial_receive_thread);
mborchers 10:f92664a22d02 128 Thread hearbeatThread(hearbeat_thread);
mborchers 0:8a6003b8bb5b 129
OWenzel 14:48cdd880ca1a 130 RtosTimer machine_direction_control_timer(redirect_machine_direction_controller);
mborchers 4:f0be27a5a83a 131 RtosTimer quadrature_control_timer(redirect_quadrature_controller);
mborchers 0:8a6003b8bb5b 132
mborchers 0:8a6003b8bb5b 133 // Konfiguration AMF-IMU
mborchers 0:8a6003b8bb5b 134 // [0]: Conversation Factor
mborchers 0:8a6003b8bb5b 135 // [1]: Sensor Position X
mborchers 0:8a6003b8bb5b 136 // [2]: Sensor Position Y
mborchers 0:8a6003b8bb5b 137 // [3]: Sensor Position Angle
torstenwylegala 5:4319a748181a 138 float configData[4] = {0.002751114f, 167.0f, 0.0f, 271.5f};
mborchers 13:34f7f783ad24 139
mborchers 13:34f7f783ad24 140 supportSystem->writeImuConfig(configData);
mborchers 0:8a6003b8bb5b 141
mborchers 0:8a6003b8bb5b 142 bool timer_started = false;
mborchers 0:8a6003b8bb5b 143
mborchers 12:221c9e02ea96 144 steering_angle_minnow_queue = 0;
mborchers 2:bf739d7d9f8f 145 quadrature_queue.put(&steering_angle_minnow_queue);
mborchers 2:bf739d7d9f8f 146
mborchers 12:221c9e02ea96 147 velocity_minnow_queue = 0;
mborchers 2:bf739d7d9f8f 148 machine_direction_queue.put(&velocity_minnow_queue);
mborchers 12:221c9e02ea96 149
mborchers 12:221c9e02ea96 150 machine_direction_control_timer.start(10);
mborchers 12:221c9e02ea96 151 quadrature_control_timer.start(10);
mborchers 0:8a6003b8bb5b 152
mborchers 0:8a6003b8bb5b 153 while(true) {
mborchers 15:141de2b5646d 154 modus_struct.identifier = 'm';
mborchers 15:141de2b5646d 155 modus_struct.payload = 1;
mborchers 15:141de2b5646d 156 serial_output_modus_queue.put(&modus_struct);
mborchers 15:141de2b5646d 157
mborchers 0:8a6003b8bb5b 158 IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer();
mborchers 0:8a6003b8bb5b 159 RadioDecoder_registerDataBuffer = supportSystem->getRadioDecoderRegisterDataBuffer();
mborchers 0:8a6003b8bb5b 160
mborchers 0:8a6003b8bb5b 161 uint16_t rc_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[0];
mborchers 0:8a6003b8bb5b 162 uint8_t rc_valid = RadioDecoder_registerDataBuffer->channelValid[0];
mborchers 0:8a6003b8bb5b 163
mborchers 0:8a6003b8bb5b 164 uint16_t drive_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[1];
mborchers 0:8a6003b8bb5b 165 uint8_t drive_valid = RadioDecoder_registerDataBuffer->channelValid[1];
mborchers 0:8a6003b8bb5b 166
mborchers 0:8a6003b8bb5b 167 uint16_t steer_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[2];
mborchers 0:8a6003b8bb5b 168 uint8_t steer_valid = RadioDecoder_registerDataBuffer->channelValid[2];
mborchers 0:8a6003b8bb5b 169
mborchers 0:8a6003b8bb5b 170
mborchers 0:8a6003b8bb5b 171 if (rc_percentage > (uint16_t) 1800 && rc_valid != 0) {
mborchers 0:8a6003b8bb5b 172 // oben => Wettbewerb
mborchers 0:8a6003b8bb5b 173 supportSystem->setLightManagerRemoteLight(false, true);
mborchers 15:141de2b5646d 174 //serialMinnow.putc('w');
mborchers 0:8a6003b8bb5b 175 if (!timer_started) {
mborchers 0:8a6003b8bb5b 176 timer_started = true;
mborchers 0:8a6003b8bb5b 177 machine_direction_control_timer.start(10);
mborchers 0:8a6003b8bb5b 178 quadrature_control_timer.start(10);
mborchers 0:8a6003b8bb5b 179 }
mborchers 0:8a6003b8bb5b 180 } else if (rc_percentage > (uint16_t) 1200 && rc_valid != 0) {
mborchers 0:8a6003b8bb5b 181 // unten => RC-Wettbewerb
mborchers 0:8a6003b8bb5b 182 supportSystem->setLightManagerRemoteLight(true, true);
mborchers 15:141de2b5646d 183 //serialMinnow.putc('r');
mborchers 0:8a6003b8bb5b 184 if (drive_valid) {
mborchers 0:8a6003b8bb5b 185 drivePWM.pulsewidth_us(drive_percentage);
mborchers 0:8a6003b8bb5b 186 }
mborchers 0:8a6003b8bb5b 187 if (steer_valid) {
mborchers 0:8a6003b8bb5b 188 steerPWM.pulsewidth_us(steer_percentage);
mborchers 0:8a6003b8bb5b 189 }
mborchers 0:8a6003b8bb5b 190 if (timer_started) {
mborchers 0:8a6003b8bb5b 191 timer_started = false;
mborchers 0:8a6003b8bb5b 192 machine_direction_control_timer.stop();
mborchers 0:8a6003b8bb5b 193 quadrature_control_timer.stop();
mborchers 0:8a6003b8bb5b 194 }
mborchers 0:8a6003b8bb5b 195 } else if (rc_percentage > (uint16_t) 800 && rc_valid != 0) {
mborchers 0:8a6003b8bb5b 196 // mitte => RC-Training
mborchers 0:8a6003b8bb5b 197 supportSystem->setLightManagerRemoteLight(true, true);
mborchers 15:141de2b5646d 198 //serialMinnow.putc('r');
mborchers 0:8a6003b8bb5b 199 if (drive_valid) {
mborchers 0:8a6003b8bb5b 200 drivePWM.pulsewidth_us(drive_percentage);
mborchers 0:8a6003b8bb5b 201 }
mborchers 0:8a6003b8bb5b 202 if (steer_valid) {
mborchers 0:8a6003b8bb5b 203 steerPWM.pulsewidth_us(steer_percentage);
mborchers 0:8a6003b8bb5b 204 }
mborchers 0:8a6003b8bb5b 205 if (timer_started) {
mborchers 0:8a6003b8bb5b 206 timer_started = false;
mborchers 0:8a6003b8bb5b 207 machine_direction_control_timer.stop();
mborchers 0:8a6003b8bb5b 208 quadrature_control_timer.stop();
mborchers 0:8a6003b8bb5b 209 }
mborchers 0:8a6003b8bb5b 210 }
mborchers 0:8a6003b8bb5b 211
mborchers 0:8a6003b8bb5b 212 velocity_imu_queue = IMU_registerDataBuffer->velocityXFilteredRegister;
mborchers 0:8a6003b8bb5b 213 imu_queue_velocity.put(&velocity_imu_queue);
mborchers 0:8a6003b8bb5b 214
mborchers 0:8a6003b8bb5b 215 float radius = IMU_registerDataBuffer->velocityXFilteredRegister/IMU_registerDataBuffer->velocityAngularFilteredRegister;
mborchers 0:8a6003b8bb5b 216
mborchers 0:8a6003b8bb5b 217 steering_angle_imu_queue = atan(0.205/radius)*180/PI;
mborchers 0:8a6003b8bb5b 218 imu_queue_steering_angle.put(&steering_angle_imu_queue);
mborchers 0:8a6003b8bb5b 219
mborchers 0:8a6003b8bb5b 220
mborchers 2:bf739d7d9f8f 221 //serialMinnow.printf("%ld, ", difference_millis);
mborchers 7:c5940ae7773a 222 //serialMinnow.printf("%f, ", velocity_minnow_queue);
mborchers 7:c5940ae7773a 223 //serialMinnow.printf("%f, ", steering_angle_minnow_queue);
mborchers 7:c5940ae7773a 224 //serialMinnow.printf("%d, ", IMU_registerDataBuffer->curveRadiusRegister);
mborchers 7:c5940ae7773a 225 //serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityAngularRegister);
mborchers 7:c5940ae7773a 226 //serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityAngularFilteredRegister);
mborchers 7:c5940ae7773a 227 //serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityXRegister);
mborchers 7:c5940ae7773a 228 //serialMinnow.printf("%f\r\n", IMU_registerDataBuffer->velocityXFilteredRegister);
mborchers 13:34f7f783ad24 229 Thread::wait(10);
mborchers 0:8a6003b8bb5b 230 }
mborchers 0:8a6003b8bb5b 231 }
mborchers 10:f92664a22d02 232
mborchers 10:f92664a22d02 233 void hearbeat_thread(void const *args) {
mborchers 10:f92664a22d02 234 while(true) {
mborchers 13:34f7f783ad24 235 heartbeatLED = !heartbeatLED;
mborchers 10:f92664a22d02 236 Thread::wait(100);
mborchers 10:f92664a22d02 237 }
mborchers 10:f92664a22d02 238 }