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@16:a387f2e275c8, 2016-02-07 (annotated)

Committer:
mborchers
Date:
Sun Feb 07 20:54:58 2016 +0000
Revision:
16:a387f2e275c8
Parent:
15:141de2b5646d
Child:
17:76636aaf80de
hjhkj

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