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@2:bf739d7d9f8f, 2016-02-03 (annotated)

Committer:
mborchers
Date:
Wed Feb 03 20:09:14 2016 +0000
Revision:
2:bf739d7d9f8f
Parent:
1:7eddde9fba60
Child:
3:391c4639bc7d
Querregelung implementiert (keine korrekte Funktion)

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