Oliver Wenzel / Mbed 2 deprecated mbed_amf_controlsystem_iO

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 by Matti Borchers

main.cpp@12:221c9e02ea96, 2016-02-04 (annotated)

Committer:
mborchers
Date:
Thu Feb 04 21:43:08 2016 +0000
Revision:
12:221c9e02ea96
Parent:
11:8b79f6545628
Child:
13:34f7f783ad24
L?uft bis auf Start des Timers

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 0:8a6003b8bb5b 6
mborchers 0:8a6003b8bb5b 7 #define PI 3.14159265
mborchers 0:8a6003b8bb5b 8
mborchers 12:221c9e02ea96 9 Serial serialXbee(p28,p27);
mborchers 0:8a6003b8bb5b 10 Serial serialMinnow(p13, p14);
mborchers 0:8a6003b8bb5b 11 PwmOut drivePWM(p22);
mborchers 0:8a6003b8bb5b 12 PwmOut steerPWM(p21);
mborchers 0:8a6003b8bb5b 13 I2C i2c(p9, p10);
mborchers 0:8a6003b8bb5b 14
mborchers 0:8a6003b8bb5b 15 DigitalOut heartbeatLED(LED1);
mborchers 0:8a6003b8bb5b 16 DigitalOut buttonLED(LED2);
mborchers 0:8a6003b8bb5b 17 DigitalOut redlightLED(LED3);
mborchers 10:f92664a22d02 18 DigitalOut led4(LED4);
mborchers 0:8a6003b8bb5b 19
mborchers 0:8a6003b8bb5b 20 DigitalIn buttonOne(p25);
mborchers 0:8a6003b8bb5b 21 DigitalIn buttonTwo(p26);
mborchers 0:8a6003b8bb5b 22 DigitalIn buttonThree(p29);
mborchers 0:8a6003b8bb5b 23 DigitalIn buttonFour(p30);
mborchers 0:8a6003b8bb5b 24
mborchers 0:8a6003b8bb5b 25 IMU_RegisterDataBuffer_t *IMU_registerDataBuffer;
mborchers 0:8a6003b8bb5b 26 RadioDecoder_RegisterDataBuffer_t *RadioDecoder_registerDataBuffer;
mborchers 0:8a6003b8bb5b 27
mborchers 4:f0be27a5a83a 28 QuadratureController *quadratureController;
mborchers 4:f0be27a5a83a 29
mborchers 0:8a6003b8bb5b 30 // Queues von der Bahnplanung
mborchers 0:8a6003b8bb5b 31 Queue<float, 2> quadrature_queue;
mborchers 0:8a6003b8bb5b 32 Queue<float, 2> machine_direction_queue;
mborchers 0:8a6003b8bb5b 33 float steering_angle_minnow_queue;
mborchers 0:8a6003b8bb5b 34 float velocity_minnow_queue;
mborchers 0:8a6003b8bb5b 35
mborchers 0:8a6003b8bb5b 36
mborchers 0:8a6003b8bb5b 37 // Queues von dem Maussensor
mborchers 0:8a6003b8bb5b 38 Queue<float, 2> imu_queue_velocity;
mborchers 0:8a6003b8bb5b 39 Queue<float, 2> imu_queue_steering_angle;
mborchers 0:8a6003b8bb5b 40 float steering_angle_imu_queue;
mborchers 0:8a6003b8bb5b 41 float velocity_imu_queue;
mborchers 0:8a6003b8bb5b 42
mborchers 0:8a6003b8bb5b 43
mborchers 0:8a6003b8bb5b 44 // Variablen von der Trajektorienplanung
mborchers 0:8a6003b8bb5b 45
mborchers 3:391c4639bc7d 46 float velocity_set = 0;
mborchers 0:8a6003b8bb5b 47
mborchers 0:8a6003b8bb5b 48 // Variablen für die Längsregelung
mborchers 0:8a6003b8bb5b 49 float velocity_current = 0, velocity_last = 0;
mborchers 0:8a6003b8bb5b 50
mborchers 1:7eddde9fba60 51 uint8_t timer_velocity_sampling_time=0.01;
mborchers 12:221c9e02ea96 52 float l_esum, Vorsteuerung, PI_Regler, l_output, l_PWM, l_e,l_Kp;
mborchers 12:221c9e02ea96 53 float l_Ki=0.1*timer_velocity_sampling_time;
mborchers 1:7eddde9fba60 54
mborchers 1:7eddde9fba60 55 // Querregelung Ende
mborchers 0:8a6003b8bb5b 56
mborchers 10:f92664a22d02 57 void hearbeat_thread(void const *args);
mborchers 10:f92664a22d02 58
mborchers 0:8a6003b8bb5b 59 void serial_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 10:f92664a22d02 66 int return_value = 666;
mborchers 0:8a6003b8bb5b 67 while (true) {
mborchers 10:f92664a22d02 68 if (ptr != end) {
mborchers 10:f92664a22d02 69 c = serialMinnow.getc();
mborchers 10:f92664a22d02 70 *ptr++ = c;
mborchers 10:f92664a22d02 71 } else {
mborchers 10:f92664a22d02 72 ptr = (char*)receive_buffer;
mborchers 7:c5940ae7773a 73 }
mborchers 7:c5940ae7773a 74 heartbeatLED = !heartbeatLED;
mborchers 10:f92664a22d02 75
mborchers 12:221c9e02ea96 76 if (c == '\r') {
mborchers 12:221c9e02ea96 77 return_value = sscanf(receive_buffer, "[%c]=%f\r", &x, &f);
mborchers 12:221c9e02ea96 78 ptr = (char*)receive_buffer;
mborchers 12:221c9e02ea96 79 }
mborchers 10:f92664a22d02 80
mborchers 10:f92664a22d02 81 if (x == 'v') {
mborchers 12:221c9e02ea96 82 machine_direction_queue.put(&f);
mborchers 12:221c9e02ea96 83 serialXbee.printf("v:%f\r\n", f);
mborchers 12:221c9e02ea96 84
mborchers 10:f92664a22d02 85 buttonLED = true;
mborchers 10:f92664a22d02 86 // Buffer wieder auf Anfang setzen
mborchers 10:f92664a22d02 87 ptr = (char*)receive_buffer;
mborchers 10:f92664a22d02 88 x = 22;
mborchers 10:f92664a22d02 89 } else if (x == 'g') {
mborchers 12:221c9e02ea96 90 quadrature_queue.put(&f);
mborchers 12:221c9e02ea96 91 serialXbee.printf("g:%f\r\n\r\n", f);
mborchers 10:f92664a22d02 92 redlightLED = true;
mborchers 10:f92664a22d02 93 // Buffer wieder auf Anfang setzen
mborchers 10:f92664a22d02 94 ptr = (char*)receive_buffer;
mborchers 10:f92664a22d02 95 x = 22;
mborchers 10:f92664a22d02 96 }
mborchers 10:f92664a22d02 97
mborchers 7:c5940ae7773a 98 //Thread::wait(100);
mborchers 0:8a6003b8bb5b 99 }
mborchers 0:8a6003b8bb5b 100 }
mborchers 0:8a6003b8bb5b 101
mborchers 0:8a6003b8bb5b 102 void machine_direction_control(void const *args) {
mborchers 0:8a6003b8bb5b 103 osEvent velocity_set_event = machine_direction_queue.get(0);
mborchers 0:8a6003b8bb5b 104 if (velocity_set_event.status == osEventMessage) {
mborchers 0:8a6003b8bb5b 105 velocity_set = *(float*)velocity_set_event.value.p;
mborchers 0:8a6003b8bb5b 106 }
mborchers 0:8a6003b8bb5b 107
mborchers 0:8a6003b8bb5b 108 osEvent velocity_current_event = imu_queue_velocity.get(0);
mborchers 0:8a6003b8bb5b 109 if (velocity_current_event.status == osEventMessage) {
mborchers 0:8a6003b8bb5b 110 velocity_current = *(float *)velocity_current_event.value.p;
mborchers 0:8a6003b8bb5b 111 }
mborchers 0:8a6003b8bb5b 112
mborchers 12:221c9e02ea96 113 Vorsteuerung=88.316*velocity_set+175.17;
mborchers 1:7eddde9fba60 114 l_e = velocity_set-velocity_current;
mborchers 1:7eddde9fba60 115 l_esum = l_esum + l_e;
mborchers 1:7eddde9fba60 116
mborchers 12:221c9e02ea96 117 PI_Regler =l_Kp*l_e+l_Ki * l_esum;
mborchers 1:7eddde9fba60 118
mborchers 12:221c9e02ea96 119 l_output=Vorsteuerung+PI_Regler;
mborchers 1:7eddde9fba60 120
mborchers 2:bf739d7d9f8f 121 l_PWM = 1500+l_output;
mborchers 12:221c9e02ea96 122
mborchers 12:221c9e02ea96 123 if(l_PWM<1500) {
mborchers 12:221c9e02ea96 124 l_PWM = 1500;
mborchers 12:221c9e02ea96 125 l_esum = l_esum-2*l_e;
mborchers 12:221c9e02ea96 126 } else if(l_PWM>2000) {
mborchers 12:221c9e02ea96 127 l_PWM = 2000;
mborchers 12:221c9e02ea96 128 l_esum = l_esum-2*l_e;
mborchers 12:221c9e02ea96 129 } else if(velocity_set < 0.1) {
mborchers 12:221c9e02ea96 130 l_PWM = 1500;
mborchers 2:bf739d7d9f8f 131 }
mborchers 1:7eddde9fba60 132
torstenwylegala 6:aa27bc8c58f5 133 drivePWM.pulsewidth_us(1700);
mborchers 12:221c9e02ea96 134 //drivePWM.pulsewidth_us(l_PWM);
mborchers 0:8a6003b8bb5b 135 }
mborchers 0:8a6003b8bb5b 136
mborchers 4:f0be27a5a83a 137 void redirect_quadrature_controller(void const *args) {
mborchers 4:f0be27a5a83a 138 quadratureController->cylic_control();
mborchers 4:f0be27a5a83a 139 }
mborchers 4:f0be27a5a83a 140
mborchers 0:8a6003b8bb5b 141 int main() {
mborchers 12:221c9e02ea96 142 serialXbee.baud(9600);
mborchers 0:8a6003b8bb5b 143 serialMinnow.baud(115200);
mborchers 0:8a6003b8bb5b 144
mborchers 0:8a6003b8bb5b 145 drivePWM.period_ms(20);
mborchers 0:8a6003b8bb5b 146 steerPWM.period_ms(20);
mborchers 0:8a6003b8bb5b 147
mborchers 0:8a6003b8bb5b 148 SystemTimer *millis = new SystemTimer();
mborchers 0:8a6003b8bb5b 149
mborchers 0:8a6003b8bb5b 150 SupportSystem *supportSystem = new SupportSystem(0x80, &i2c);
mborchers 3:391c4639bc7d 151
torstenwylegala 6:aa27bc8c58f5 152 quadratureController = new QuadratureController(&steerPWM, &quadrature_queue, &imu_queue_steering_angle);
mborchers 0:8a6003b8bb5b 153
mborchers 1:7eddde9fba60 154 Thread machineDirectionControl(serial_thread);
mborchers 10:f92664a22d02 155 Thread hearbeatThread(hearbeat_thread);
mborchers 0:8a6003b8bb5b 156
mborchers 0:8a6003b8bb5b 157 RtosTimer machine_direction_control_timer(machine_direction_control);
mborchers 4:f0be27a5a83a 158 RtosTimer quadrature_control_timer(redirect_quadrature_controller);
mborchers 0:8a6003b8bb5b 159
mborchers 0:8a6003b8bb5b 160 // Konfiguration AMF-IMU
mborchers 0:8a6003b8bb5b 161 // [0]: Conversation Factor
mborchers 0:8a6003b8bb5b 162 // [1]: Sensor Position X
mborchers 0:8a6003b8bb5b 163 // [2]: Sensor Position Y
mborchers 0:8a6003b8bb5b 164 // [3]: Sensor Position Angle
torstenwylegala 5:4319a748181a 165 float configData[4] = {0.002751114f, 167.0f, 0.0f, 271.5f};
mborchers 0:8a6003b8bb5b 166
mborchers 0:8a6003b8bb5b 167 supportSystem->writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_CONVERSION_FACTOR, configData, sizeof(float)*4);
mborchers 0:8a6003b8bb5b 168
mborchers 0:8a6003b8bb5b 169 // Flag setzen
torstenwylegala 5:4319a748181a 170 uint8_t command = 1<<2;
mborchers 0:8a6003b8bb5b 171 supportSystem->writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_COMMAND, &command, sizeof(uint8_t));
mborchers 0:8a6003b8bb5b 172
mborchers 0:8a6003b8bb5b 173 bool timer_started = false;
mborchers 0:8a6003b8bb5b 174
mborchers 0:8a6003b8bb5b 175 wait(0.1);
mborchers 0:8a6003b8bb5b 176
mborchers 12:221c9e02ea96 177 steering_angle_minnow_queue = 0;
mborchers 2:bf739d7d9f8f 178 quadrature_queue.put(&steering_angle_minnow_queue);
mborchers 2:bf739d7d9f8f 179
mborchers 12:221c9e02ea96 180 velocity_minnow_queue = 0;
mborchers 2:bf739d7d9f8f 181 machine_direction_queue.put(&velocity_minnow_queue);
mborchers 12:221c9e02ea96 182
mborchers 12:221c9e02ea96 183 machine_direction_control_timer.start(10);
mborchers 12:221c9e02ea96 184 quadrature_control_timer.start(10);
mborchers 0:8a6003b8bb5b 185
mborchers 0:8a6003b8bb5b 186 while(true) {
mborchers 0:8a6003b8bb5b 187 IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer();
mborchers 0:8a6003b8bb5b 188 RadioDecoder_registerDataBuffer = supportSystem->getRadioDecoderRegisterDataBuffer();
mborchers 0:8a6003b8bb5b 189
mborchers 7:c5940ae7773a 190 //for(uint8_t i=0; i<3; i++) {
mborchers 7:c5940ae7773a 191 // serialMinnow.printf("RadioDecoder - Ch[%d] Valid: %d\r\n",i,RadioDecoder_registerDataBuffer->channelValid[i]);
mborchers 7:c5940ae7773a 192 // serialMinnow.printf("RadioDecoder - Ch[%d] ActiveTime: %d\r\n",i,RadioDecoder_registerDataBuffer->channelActiveTime[i]);
mborchers 7:c5940ae7773a 193 // serialMinnow.printf("RadioDecoder - Ch[%d] Percentage: %d\r\n\r\n",i,RadioDecoder_registerDataBuffer->channelPercent[i]);
mborchers 7:c5940ae7773a 194 //}
mborchers 0:8a6003b8bb5b 195
mborchers 0:8a6003b8bb5b 196 uint16_t rc_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[0];
mborchers 0:8a6003b8bb5b 197 uint8_t rc_valid = RadioDecoder_registerDataBuffer->channelValid[0];
mborchers 0:8a6003b8bb5b 198
mborchers 0:8a6003b8bb5b 199 uint16_t drive_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[1];
mborchers 0:8a6003b8bb5b 200 uint8_t drive_valid = RadioDecoder_registerDataBuffer->channelValid[1];
mborchers 0:8a6003b8bb5b 201
mborchers 0:8a6003b8bb5b 202 uint16_t steer_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[2];
mborchers 0:8a6003b8bb5b 203 uint8_t steer_valid = RadioDecoder_registerDataBuffer->channelValid[2];
mborchers 0:8a6003b8bb5b 204
mborchers 0:8a6003b8bb5b 205
mborchers 0:8a6003b8bb5b 206 if (rc_percentage > (uint16_t) 1800 && rc_valid != 0) {
mborchers 0:8a6003b8bb5b 207 // oben => Wettbewerb
mborchers 7:c5940ae7773a 208 //heartbeatLED = true;
mborchers 10:f92664a22d02 209 //buttonLED = false;
mborchers 10:f92664a22d02 210 //redlightLED = false;
mborchers 0:8a6003b8bb5b 211 supportSystem->setLightManagerRemoteLight(false, true);
mborchers 0:8a6003b8bb5b 212 if (!timer_started) {
mborchers 0:8a6003b8bb5b 213 timer_started = true;
mborchers 0:8a6003b8bb5b 214 machine_direction_control_timer.start(10);
mborchers 0:8a6003b8bb5b 215 quadrature_control_timer.start(10);
mborchers 0:8a6003b8bb5b 216 }
mborchers 0:8a6003b8bb5b 217 } else if (rc_percentage > (uint16_t) 1200 && rc_valid != 0) {
mborchers 0:8a6003b8bb5b 218 // unten => RC-Wettbewerb
mborchers 7:c5940ae7773a 219 //heartbeatLED = false;
mborchers 10:f92664a22d02 220 //buttonLED = false;
mborchers 10:f92664a22d02 221 //redlightLED = true;
mborchers 0:8a6003b8bb5b 222 supportSystem->setLightManagerRemoteLight(true, true);
mborchers 0:8a6003b8bb5b 223 if (drive_valid) {
mborchers 0:8a6003b8bb5b 224 drivePWM.pulsewidth_us(drive_percentage);
mborchers 0:8a6003b8bb5b 225 }
mborchers 0:8a6003b8bb5b 226 if (steer_valid) {
mborchers 0:8a6003b8bb5b 227 steerPWM.pulsewidth_us(steer_percentage);
mborchers 0:8a6003b8bb5b 228 }
mborchers 0:8a6003b8bb5b 229 if (timer_started) {
mborchers 0:8a6003b8bb5b 230 timer_started = false;
mborchers 0:8a6003b8bb5b 231 machine_direction_control_timer.stop();
mborchers 0:8a6003b8bb5b 232 quadrature_control_timer.stop();
mborchers 0:8a6003b8bb5b 233 }
mborchers 0:8a6003b8bb5b 234 } else if (rc_percentage > (uint16_t) 800 && rc_valid != 0) {
mborchers 0:8a6003b8bb5b 235 // mitte => RC-Training
mborchers 7:c5940ae7773a 236 //heartbeatLED = false;
mborchers 10:f92664a22d02 237 //buttonLED = true;
mborchers 10:f92664a22d02 238 //redlightLED = false;
mborchers 0:8a6003b8bb5b 239 supportSystem->setLightManagerRemoteLight(true, true);
mborchers 0:8a6003b8bb5b 240 if (drive_valid) {
mborchers 0:8a6003b8bb5b 241 drivePWM.pulsewidth_us(drive_percentage);
mborchers 0:8a6003b8bb5b 242 }
mborchers 0:8a6003b8bb5b 243 if (steer_valid) {
mborchers 0:8a6003b8bb5b 244 steerPWM.pulsewidth_us(steer_percentage);
mborchers 0:8a6003b8bb5b 245 }
mborchers 0:8a6003b8bb5b 246 if (timer_started) {
mborchers 0:8a6003b8bb5b 247 timer_started = false;
mborchers 0:8a6003b8bb5b 248 machine_direction_control_timer.stop();
mborchers 0:8a6003b8bb5b 249 quadrature_control_timer.stop();
mborchers 0:8a6003b8bb5b 250 }
mborchers 0:8a6003b8bb5b 251 }
mborchers 0:8a6003b8bb5b 252
mborchers 0:8a6003b8bb5b 253 velocity_imu_queue = IMU_registerDataBuffer->velocityXFilteredRegister;
mborchers 0:8a6003b8bb5b 254 imu_queue_velocity.put(&velocity_imu_queue);
mborchers 0:8a6003b8bb5b 255
mborchers 0:8a6003b8bb5b 256 float radius = IMU_registerDataBuffer->velocityXFilteredRegister/IMU_registerDataBuffer->velocityAngularFilteredRegister;
mborchers 0:8a6003b8bb5b 257
mborchers 0:8a6003b8bb5b 258 steering_angle_imu_queue = atan(0.205/radius)*180/PI;
mborchers 0:8a6003b8bb5b 259 imu_queue_steering_angle.put(&steering_angle_imu_queue);
mborchers 0:8a6003b8bb5b 260
mborchers 0:8a6003b8bb5b 261
mborchers 2:bf739d7d9f8f 262 //serialMinnow.printf("%ld, ", difference_millis);
mborchers 7:c5940ae7773a 263 //serialMinnow.printf("%f, ", velocity_minnow_queue);
mborchers 7:c5940ae7773a 264 //serialMinnow.printf("%f, ", steering_angle_minnow_queue);
mborchers 7:c5940ae7773a 265 //serialMinnow.printf("%d, ", IMU_registerDataBuffer->curveRadiusRegister);
mborchers 7:c5940ae7773a 266 //serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityAngularRegister);
mborchers 7:c5940ae7773a 267 //serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityAngularFilteredRegister);
mborchers 7:c5940ae7773a 268 //serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityXRegister);
mborchers 7:c5940ae7773a 269 //serialMinnow.printf("%f\r\n", IMU_registerDataBuffer->velocityXFilteredRegister);
mborchers 0:8a6003b8bb5b 270 Thread::wait(50);
mborchers 0:8a6003b8bb5b 271 }
mborchers 0:8a6003b8bb5b 272 }
mborchers 10:f92664a22d02 273
mborchers 10:f92664a22d02 274 void hearbeat_thread(void const *args) {
mborchers 10:f92664a22d02 275 while(true) {
mborchers 10:f92664a22d02 276 led4 = !led4;
mborchers 10:f92664a22d02 277 Thread::wait(100);
mborchers 10:f92664a22d02 278 }
mborchers 10:f92664a22d02 279 }