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@17:76636aaf80de, 2016-02-08 (annotated)

Committer:
mborchers
Date:
Mon Feb 08 14:56:34 2016 +0000
Revision:
17:76636aaf80de
Parent:
16:a387f2e275c8
Child:
18:0e8ad413a840
Parken implementiert (nicht lauff?hig)

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 17:76636aaf80de 7 #include "Modus/Parking.h"
mborchers 0:8a6003b8bb5b 8
mborchers 0:8a6003b8bb5b 9 #define PI 3.14159265
mborchers 0:8a6003b8bb5b 10
mborchers 17:76636aaf80de 11
mborchers 17:76636aaf80de 12 // Different Modes für the serial protocol
mborchers 17:76636aaf80de 13 static const uint8_t MODUS_PARKING = 1, MODUS_DRIVING = 2, MODUS_DRIVING_OBSTACLE = 3, MODUS_RC = 4, MODUS_NO_RC = 5;
mborchers 17:76636aaf80de 14
mborchers 17:76636aaf80de 15 static const uint8_t INIT_STATE = 0, DRIVE_STATE = 1, PARKING_STATE = 2;
mborchers 17:76636aaf80de 16 uint8_t current_state = INIT_STATE;
mborchers 17:76636aaf80de 17
mborchers 13:34f7f783ad24 18 Serial serialXbee(p28,p27), serialMinnow(p13, p14);
mborchers 13:34f7f783ad24 19 PwmOut drivePWM(p22), steerPWM(p21);
mborchers 0:8a6003b8bb5b 20 I2C i2c(p9, p10);
mborchers 0:8a6003b8bb5b 21
mborchers 13:34f7f783ad24 22 DigitalOut serialLED(LED1), ledTwo(LED2), ledThree(LED3), heartbeatLED(LED4);
mborchers 0:8a6003b8bb5b 23
mborchers 17:76636aaf80de 24 DigitalIn buttonOne(p25), buttonDrivingObstacle(p26), buttonDriving(p29), buttonParking(p30);
mborchers 0:8a6003b8bb5b 25
mborchers 0:8a6003b8bb5b 26 IMU_RegisterDataBuffer_t *IMU_registerDataBuffer;
mborchers 0:8a6003b8bb5b 27 RadioDecoder_RegisterDataBuffer_t *RadioDecoder_registerDataBuffer;
mborchers 0:8a6003b8bb5b 28
mborchers 4:f0be27a5a83a 29 QuadratureController *quadratureController;
mborchers 13:34f7f783ad24 30 MachineDirectionController *machineDirectionController;
mborchers 4:f0be27a5a83a 31
mborchers 0:8a6003b8bb5b 32 // Queues von der Bahnplanung
mborchers 0:8a6003b8bb5b 33 Queue<float, 2> quadrature_queue;
mborchers 0:8a6003b8bb5b 34 Queue<float, 2> machine_direction_queue;
mborchers 0:8a6003b8bb5b 35 float steering_angle_minnow_queue;
mborchers 0:8a6003b8bb5b 36 float velocity_minnow_queue;
mborchers 0:8a6003b8bb5b 37
mborchers 0:8a6003b8bb5b 38
mborchers 0:8a6003b8bb5b 39 // Queues von dem Maussensor
mborchers 0:8a6003b8bb5b 40 Queue<float, 2> imu_queue_velocity;
mborchers 0:8a6003b8bb5b 41 Queue<float, 2> imu_queue_steering_angle;
mborchers 0:8a6003b8bb5b 42 float steering_angle_imu_queue;
mborchers 0:8a6003b8bb5b 43 float velocity_imu_queue;
mborchers 0:8a6003b8bb5b 44
mborchers 17:76636aaf80de 45 // Queue
mborchers 15:141de2b5646d 46 typedef struct {
mborchers 15:141de2b5646d 47 char identifier;
mborchers 15:141de2b5646d 48 uint8_t payload;
mborchers 15:141de2b5646d 49 } serial_output_t;
mborchers 16:a387f2e275c8 50 //serial_output_t modus_struct;
mborchers 16:a387f2e275c8 51 //Queue<serial_output_t, 2> serial_output_modus_queue;
mborchers 16:a387f2e275c8 52 uint8_t modus;
mborchers 16:a387f2e275c8 53 Queue<uint8_t, 2> serial_output_modus_queue;
mborchers 16:a387f2e275c8 54
mborchers 17:76636aaf80de 55 uint8_t current_modus;
mborchers 17:76636aaf80de 56 Queue<uint8_t, 2> serial_input_modus_queue;
mborchers 17:76636aaf80de 57
mborchers 17:76636aaf80de 58
mborchers 17:76636aaf80de 59 InterruptIn lightSensor(p12);
mborchers 15:141de2b5646d 60
mborchers 10:f92664a22d02 61 void hearbeat_thread(void const *args);
mborchers 10:f92664a22d02 62
mborchers 17:76636aaf80de 63 void serial_transmit_thread(void const *args)
mborchers 17:76636aaf80de 64 {
mborchers 15:141de2b5646d 65 osEvent modus_event;
mborchers 17:76636aaf80de 66 uint8_t received_modus;
mborchers 15:141de2b5646d 67 while (true) {
mborchers 16:a387f2e275c8 68 modus_event = serial_output_modus_queue.get();
mborchers 15:141de2b5646d 69 if (modus_event.status == osEventMessage) {
mborchers 17:76636aaf80de 70 received_modus = *((uint8_t *)modus_event.value.p);
mborchers 17:76636aaf80de 71 if (received_modus == MODUS_PARKING) {
mborchers 17:76636aaf80de 72 serialMinnow.printf("[m]=%d\r", received_modus);
mborchers 17:76636aaf80de 73 } else if (received_modus == MODUS_DRIVING) {
mborchers 17:76636aaf80de 74 serialMinnow.printf("[m]=%d\r", received_modus);
mborchers 17:76636aaf80de 75 } else if (received_modus == MODUS_DRIVING_OBSTACLE) {
mborchers 17:76636aaf80de 76 serialMinnow.printf("[m]=%d\r", received_modus);
mborchers 17:76636aaf80de 77 } else if (received_modus == MODUS_RC) {
mborchers 17:76636aaf80de 78 serialMinnow.printf("[r]=1\r");
mborchers 17:76636aaf80de 79 } else if (received_modus == MODUS_NO_RC) {
mborchers 17:76636aaf80de 80 serialMinnow.printf("[r]=0\r");
mborchers 17:76636aaf80de 81 }
mborchers 15:141de2b5646d 82 }
mborchers 15:141de2b5646d 83 }
mborchers 15:141de2b5646d 84 }
mborchers 15:141de2b5646d 85
mborchers 15:141de2b5646d 86 void serial_receive_thread(void const *args) {
mborchers 10:f92664a22d02 87 size_t length = 20;
mborchers 10:f92664a22d02 88 char receive_buffer[length];
mborchers 10:f92664a22d02 89 char* ptr = (char*) receive_buffer;
mborchers 10:f92664a22d02 90 char* end = ptr + length;
mborchers 10:f92664a22d02 91 char c, x = 22;
mborchers 7:c5940ae7773a 92 float f;
mborchers 17:76636aaf80de 93 osEvent modus_event;
mborchers 17:76636aaf80de 94 uint8_t received_modus = MODUS_NO_RC;
mborchers 0:8a6003b8bb5b 95 while (true) {
mborchers 16:a387f2e275c8 96 while (serialMinnow.readable()) {
mborchers 16:a387f2e275c8 97 if (ptr != end) {
mborchers 16:a387f2e275c8 98 c = serialMinnow.getc();
mborchers 16:a387f2e275c8 99 *ptr++ = c;
mborchers 16:a387f2e275c8 100 } else {
mborchers 16:a387f2e275c8 101 ptr = (char*)receive_buffer;
mborchers 16:a387f2e275c8 102 }
mborchers 16:a387f2e275c8 103 serialLED = !serialLED;
mborchers 16:a387f2e275c8 104 if (c == '\r') {
mborchers 16:a387f2e275c8 105 sscanf(receive_buffer, "[%c]=%f\r", &x, &f);
mborchers 16:a387f2e275c8 106 ptr = (char*)receive_buffer;
mborchers 16:a387f2e275c8 107 }
mborchers 17:76636aaf80de 108
mborchers 17:76636aaf80de 109 modus_event = serial_output_modus_queue.get(0);
mborchers 17:76636aaf80de 110 if (modus_event.status == osEventMessage) {
mborchers 17:76636aaf80de 111 received_modus = *((uint8_t *)modus_event.value.p);
mborchers 17:76636aaf80de 112 }
mborchers 17:76636aaf80de 113
mborchers 16:a387f2e275c8 114 if (x == 'v') {
mborchers 17:76636aaf80de 115 if (received_modus != MODUS_NO_RC) {
mborchers 17:76636aaf80de 116 machine_direction_queue.put(&f);
mborchers 17:76636aaf80de 117 }
mborchers 17:76636aaf80de 118
mborchers 16:a387f2e275c8 119 ledTwo = true;
mborchers 16:a387f2e275c8 120 // Buffer wieder auf Anfang setzen
mborchers 16:a387f2e275c8 121 ptr = (char*)receive_buffer;
mborchers 16:a387f2e275c8 122 x = 22;
mborchers 16:a387f2e275c8 123 } else if (x == 'g') {
mborchers 17:76636aaf80de 124 if (received_modus != MODUS_NO_RC) {
mborchers 17:76636aaf80de 125 f = -1*f;
mborchers 17:76636aaf80de 126 quadrature_queue.put(&f);
mborchers 17:76636aaf80de 127 }
mborchers 16:a387f2e275c8 128 ledThree = true;
mborchers 16:a387f2e275c8 129 // Buffer wieder auf Anfang setzen
mborchers 16:a387f2e275c8 130 ptr = (char*)receive_buffer;
mborchers 16:a387f2e275c8 131 x = 22;
mborchers 16:a387f2e275c8 132 }
mborchers 12:221c9e02ea96 133 }
mborchers 17:76636aaf80de 134
mborchers 16:a387f2e275c8 135 Thread::wait(10);
mborchers 0:8a6003b8bb5b 136 }
mborchers 0:8a6003b8bb5b 137 }
mborchers 0:8a6003b8bb5b 138
mborchers 17:76636aaf80de 139 void redirect_machine_direction_controller(void const *args)
mborchers 17:76636aaf80de 140 {
mborchers 13:34f7f783ad24 141 machineDirectionController->cylic_control();
mborchers 0:8a6003b8bb5b 142 }
mborchers 0:8a6003b8bb5b 143
mborchers 17:76636aaf80de 144 void redirect_quadrature_controller(void const *args)
mborchers 17:76636aaf80de 145 {
mborchers 4:f0be27a5a83a 146 quadratureController->cylic_control();
mborchers 4:f0be27a5a83a 147 }
mborchers 4:f0be27a5a83a 148
mborchers 17:76636aaf80de 149 int main()
mborchers 17:76636aaf80de 150 {
mborchers 0:8a6003b8bb5b 151 serialMinnow.baud(115200);
mborchers 0:8a6003b8bb5b 152
mborchers 0:8a6003b8bb5b 153 drivePWM.period_ms(20);
mborchers 0:8a6003b8bb5b 154 steerPWM.period_ms(20);
mborchers 0:8a6003b8bb5b 155
mborchers 0:8a6003b8bb5b 156 SystemTimer *millis = new SystemTimer();
mborchers 0:8a6003b8bb5b 157
mborchers 0:8a6003b8bb5b 158 SupportSystem *supportSystem = new SupportSystem(0x80, &i2c);
mborchers 17:76636aaf80de 159
torstenwylegala 6:aa27bc8c58f5 160 quadratureController = new QuadratureController(&steerPWM, &quadrature_queue, &imu_queue_steering_angle);
mborchers 13:34f7f783ad24 161 machineDirectionController = new MachineDirectionController(&drivePWM, &machine_direction_queue, &imu_queue_velocity);
mborchers 17:76636aaf80de 162
mborchers 17:76636aaf80de 163 lightSensor.rise(&Parking::increaseLightimpulseCounter);
mborchers 17:76636aaf80de 164
mborchers 16:a387f2e275c8 165 Thread serialTransmitThread(serial_transmit_thread);
mborchers 15:141de2b5646d 166 Thread machineDirectionControl(serial_receive_thread);
mborchers 10:f92664a22d02 167 Thread hearbeatThread(hearbeat_thread);
mborchers 17:76636aaf80de 168 Thread testParkingThread(Parking::parking_thread);
mborchers 17:76636aaf80de 169
OWenzel 14:48cdd880ca1a 170 RtosTimer machine_direction_control_timer(redirect_machine_direction_controller);
mborchers 4:f0be27a5a83a 171 RtosTimer quadrature_control_timer(redirect_quadrature_controller);
mborchers 0:8a6003b8bb5b 172
mborchers 0:8a6003b8bb5b 173 // Konfiguration AMF-IMU
mborchers 0:8a6003b8bb5b 174 // [0]: Conversation Factor
mborchers 0:8a6003b8bb5b 175 // [1]: Sensor Position X
mborchers 0:8a6003b8bb5b 176 // [2]: Sensor Position Y
mborchers 0:8a6003b8bb5b 177 // [3]: Sensor Position Angle
torstenwylegala 5:4319a748181a 178 float configData[4] = {0.002751114f, 167.0f, 0.0f, 271.5f};
mborchers 17:76636aaf80de 179
mborchers 13:34f7f783ad24 180 supportSystem->writeImuConfig(configData);
mborchers 17:76636aaf80de 181
mborchers 12:221c9e02ea96 182 steering_angle_minnow_queue = 0;
mborchers 2:bf739d7d9f8f 183 quadrature_queue.put(&steering_angle_minnow_queue);
mborchers 17:76636aaf80de 184
mborchers 12:221c9e02ea96 185 velocity_minnow_queue = 0;
mborchers 2:bf739d7d9f8f 186 machine_direction_queue.put(&velocity_minnow_queue);
mborchers 17:76636aaf80de 187
mborchers 12:221c9e02ea96 188 machine_direction_control_timer.start(10);
mborchers 12:221c9e02ea96 189 quadrature_control_timer.start(10);
mborchers 0:8a6003b8bb5b 190
mborchers 0:8a6003b8bb5b 191 while(true) {
mborchers 17:76636aaf80de 192
mborchers 0:8a6003b8bb5b 193 IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer();
mborchers 0:8a6003b8bb5b 194 RadioDecoder_registerDataBuffer = supportSystem->getRadioDecoderRegisterDataBuffer();
mborchers 0:8a6003b8bb5b 195
mborchers 17:76636aaf80de 196 uint16_t rc_active_time = RadioDecoder_registerDataBuffer->channelActiveTime[0];
mborchers 0:8a6003b8bb5b 197 uint8_t rc_valid = RadioDecoder_registerDataBuffer->channelValid[0];
mborchers 17:76636aaf80de 198
mborchers 17:76636aaf80de 199 int8_t drive_percentage = RadioDecoder_registerDataBuffer->channelPercent[1];
mborchers 0:8a6003b8bb5b 200 uint8_t drive_valid = RadioDecoder_registerDataBuffer->channelValid[1];
mborchers 0:8a6003b8bb5b 201
mborchers 17:76636aaf80de 202 int8_t steer_percentage = RadioDecoder_registerDataBuffer->channelPercent[2];
mborchers 0:8a6003b8bb5b 203 uint8_t steer_valid = RadioDecoder_registerDataBuffer->channelValid[2];
mborchers 0:8a6003b8bb5b 204
mborchers 0:8a6003b8bb5b 205
mborchers 17:76636aaf80de 206 if (rc_active_time > (uint16_t) 1800 && rc_valid != 0) {
mborchers 0:8a6003b8bb5b 207 // oben => Wettbewerb
mborchers 0:8a6003b8bb5b 208 supportSystem->setLightManagerRemoteLight(false, true);
mborchers 17:76636aaf80de 209 modus = MODUS_NO_RC;
mborchers 17:76636aaf80de 210 serial_output_modus_queue.put(&modus);
mborchers 17:76636aaf80de 211
mborchers 17:76636aaf80de 212 } else if (rc_active_time > (uint16_t) 1200 && rc_valid != 0) {
mborchers 0:8a6003b8bb5b 213 // unten => RC-Wettbewerb
mborchers 0:8a6003b8bb5b 214 supportSystem->setLightManagerRemoteLight(true, true);
mborchers 17:76636aaf80de 215 Thread::wait(1000);
mborchers 17:76636aaf80de 216 modus = MODUS_RC;
mborchers 17:76636aaf80de 217 serial_output_modus_queue.put(&modus);
mborchers 0:8a6003b8bb5b 218 if (drive_valid) {
mborchers 17:76636aaf80de 219 velocity_minnow_queue = 0.3 * drive_percentage;
mborchers 17:76636aaf80de 220 machine_direction_queue.put(&velocity_minnow_queue);
mborchers 0:8a6003b8bb5b 221 }
mborchers 0:8a6003b8bb5b 222 if (steer_valid) {
mborchers 0:8a6003b8bb5b 223 steerPWM.pulsewidth_us(steer_percentage);
mborchers 0:8a6003b8bb5b 224 }
mborchers 17:76636aaf80de 225 } else if (rc_active_time > (uint16_t) 800 && rc_valid != 0) {
mborchers 0:8a6003b8bb5b 226 // mitte => RC-Training
mborchers 0:8a6003b8bb5b 227 supportSystem->setLightManagerRemoteLight(true, true);
mborchers 17:76636aaf80de 228 modus = MODUS_RC;
mborchers 17:76636aaf80de 229 serial_output_modus_queue.put(&modus);
mborchers 0:8a6003b8bb5b 230 if (drive_valid) {
mborchers 17:76636aaf80de 231 velocity_minnow_queue = 2.0 * drive_percentage;
mborchers 17:76636aaf80de 232 machine_direction_queue.put(&velocity_minnow_queue);
mborchers 0:8a6003b8bb5b 233 }
mborchers 0:8a6003b8bb5b 234 if (steer_valid) {
mborchers 0:8a6003b8bb5b 235 steerPWM.pulsewidth_us(steer_percentage);
mborchers 0:8a6003b8bb5b 236 }
mborchers 17:76636aaf80de 237 }
mborchers 17:76636aaf80de 238
mborchers 17:76636aaf80de 239 if (current_state == INIT_STATE) {
mborchers 17:76636aaf80de 240 // Initializing State
mborchers 17:76636aaf80de 241 // TODO: Start and Initialize Controllers (velocity = 0), (gamma = 0)
mborchers 17:76636aaf80de 242 if (buttonDriving) {
mborchers 17:76636aaf80de 243 current_state = DRIVE_STATE;
mborchers 17:76636aaf80de 244 } else if (buttonDrivingObstacle) {
mborchers 17:76636aaf80de 245 // No Action until now
mborchers 17:76636aaf80de 246 } else if (buttonParking) {
mborchers 17:76636aaf80de 247 current_state = PARKING_STATE;
mborchers 0:8a6003b8bb5b 248 }
mborchers 17:76636aaf80de 249 } else if (current_state == DRIVE_STATE) {
mborchers 17:76636aaf80de 250 // Driving State
mborchers 17:76636aaf80de 251 serial_output_modus_queue.put(&current_state);
mborchers 17:76636aaf80de 252 } else if (current_state == PARKING_STATE) {
mborchers 17:76636aaf80de 253 // Parking State
mborchers 17:76636aaf80de 254 // TODO: Start Parking Thread
mborchers 17:76636aaf80de 255 serial_output_modus_queue.put(&current_state);
mborchers 0:8a6003b8bb5b 256 }
mborchers 17:76636aaf80de 257
mborchers 0:8a6003b8bb5b 258 velocity_imu_queue = IMU_registerDataBuffer->velocityXFilteredRegister;
mborchers 0:8a6003b8bb5b 259 imu_queue_velocity.put(&velocity_imu_queue);
mborchers 17:76636aaf80de 260
mborchers 0:8a6003b8bb5b 261 float radius = IMU_registerDataBuffer->velocityXFilteredRegister/IMU_registerDataBuffer->velocityAngularFilteredRegister;
mborchers 17:76636aaf80de 262
mborchers 0:8a6003b8bb5b 263 steering_angle_imu_queue = atan(0.205/radius)*180/PI;
mborchers 0:8a6003b8bb5b 264 imu_queue_steering_angle.put(&steering_angle_imu_queue);
mborchers 0:8a6003b8bb5b 265
mborchers 13:34f7f783ad24 266 Thread::wait(10);
mborchers 0:8a6003b8bb5b 267 }
mborchers 0:8a6003b8bb5b 268 }
mborchers 10:f92664a22d02 269
mborchers 17:76636aaf80de 270 void hearbeat_thread(void const *args)
mborchers 17:76636aaf80de 271 {
mborchers 10:f92664a22d02 272 while(true) {
mborchers 13:34f7f783ad24 273 heartbeatLED = !heartbeatLED;
mborchers 10:f92664a22d02 274 Thread::wait(100);
mborchers 10:f92664a22d02 275 }
mborchers 16:a387f2e275c8 276 }