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@3:391c4639bc7d, 2016-02-04 (annotated)

Committer:
mborchers
Date:
Thu Feb 04 08:54:06 2016 +0000
Revision:
3:391c4639bc7d
Parent:
2:bf739d7d9f8f
Child:
4:f0be27a5a83a
Habe versucht die Querregelung in eine Klasse auszulagern (Quadrature Control). Er wirft allerdings ein Fehler wenn ich die Funktion aus dem Klasse dem Timer ?bergeben will.

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 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 3:391c4639bc7d 42 float velocity_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 1:7eddde9fba60 53 // Querregelung Ende
mborchers 0:8a6003b8bb5b 54
mborchers 0:8a6003b8bb5b 55 void serial_thread(void const *args) {
mborchers 0:8a6003b8bb5b 56 while (true) {
mborchers 0:8a6003b8bb5b 57 Thread::wait(100);
mborchers 0:8a6003b8bb5b 58 }
mborchers 0:8a6003b8bb5b 59 }
mborchers 0:8a6003b8bb5b 60
mborchers 0:8a6003b8bb5b 61 void machine_direction_control(void const *args) {
mborchers 0:8a6003b8bb5b 62 osEvent velocity_set_event = machine_direction_queue.get(0);
mborchers 0:8a6003b8bb5b 63 if (velocity_set_event.status == osEventMessage) {
mborchers 0:8a6003b8bb5b 64 velocity_set = *(float*)velocity_set_event.value.p;
mborchers 0:8a6003b8bb5b 65 }
mborchers 0:8a6003b8bb5b 66
mborchers 0:8a6003b8bb5b 67 osEvent velocity_current_event = imu_queue_velocity.get(0);
mborchers 0:8a6003b8bb5b 68 if (velocity_current_event.status == osEventMessage) {
mborchers 0:8a6003b8bb5b 69 velocity_current = *(float *)velocity_current_event.value.p;
mborchers 0:8a6003b8bb5b 70 }
mborchers 0:8a6003b8bb5b 71
mborchers 1:7eddde9fba60 72 Vorsteuerungsfaktor = a[(uint8_t)velocity_set*4];
mborchers 1:7eddde9fba60 73 l_e = velocity_set-velocity_current;
mborchers 1:7eddde9fba60 74 l_esum = l_esum + l_e;
mborchers 1:7eddde9fba60 75
mborchers 1:7eddde9fba60 76 Vorsteuerung=Vorsteuerungsfaktor*velocity_set;
mborchers 3:391c4639bc7d 77 I_Regler = l_Ki * l_esum;
mborchers 1:7eddde9fba60 78
mborchers 2:bf739d7d9f8f 79 l_output=Vorsteuerung;//+I_Regler;
mborchers 1:7eddde9fba60 80
mborchers 2:bf739d7d9f8f 81 l_PWM = 1500+l_output;
mborchers 2:bf739d7d9f8f 82
mborchers 2:bf739d7d9f8f 83 if(l_PWM<1500)
mborchers 2:bf739d7d9f8f 84 {
mborchers 2:bf739d7d9f8f 85 l_PWM=1500;
mborchers 2:bf739d7d9f8f 86 }
mborchers 2:bf739d7d9f8f 87 else if(l_PWM>2000)
mborchers 2:bf739d7d9f8f 88 {
mborchers 2:bf739d7d9f8f 89 l_PWM=2000;
mborchers 2:bf739d7d9f8f 90 }
mborchers 1:7eddde9fba60 91
mborchers 1:7eddde9fba60 92 drivePWM.pulsewidth_us(l_PWM);
mborchers 0:8a6003b8bb5b 93 }
mborchers 0:8a6003b8bb5b 94
mborchers 0:8a6003b8bb5b 95 int main() {
mborchers 0:8a6003b8bb5b 96 serialMinnow.baud(115200);
mborchers 0:8a6003b8bb5b 97
mborchers 0:8a6003b8bb5b 98 drivePWM.period_ms(20);
mborchers 0:8a6003b8bb5b 99 steerPWM.period_ms(20);
mborchers 0:8a6003b8bb5b 100
mborchers 0:8a6003b8bb5b 101 SystemTimer *millis = new SystemTimer();
mborchers 0:8a6003b8bb5b 102
mborchers 0:8a6003b8bb5b 103 SupportSystem *supportSystem = new SupportSystem(0x80, &i2c);
mborchers 3:391c4639bc7d 104
mborchers 3:391c4639bc7d 105 QuadratureController *quadratureController = new QuadratureController(&steerPWM);
mborchers 0:8a6003b8bb5b 106
mborchers 1:7eddde9fba60 107 Thread machineDirectionControl(serial_thread);
mborchers 0:8a6003b8bb5b 108
mborchers 0:8a6003b8bb5b 109 RtosTimer machine_direction_control_timer(machine_direction_control);
mborchers 3:391c4639bc7d 110 RtosTimer quadrature_control_timer(quadratureController->cylic_control);
mborchers 0:8a6003b8bb5b 111
mborchers 0:8a6003b8bb5b 112 // Konfiguration AMF-IMU
mborchers 0:8a6003b8bb5b 113 // [0]: Conversation Factor
mborchers 0:8a6003b8bb5b 114 // [1]: Sensor Position X
mborchers 0:8a6003b8bb5b 115 // [2]: Sensor Position Y
mborchers 0:8a6003b8bb5b 116 // [3]: Sensor Position Angle
mborchers 0:8a6003b8bb5b 117 float configData[4] = {0.002751114f, 167.0f, 0.0f, 269.0f};
mborchers 0:8a6003b8bb5b 118
mborchers 0:8a6003b8bb5b 119 supportSystem->writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_CONVERSION_FACTOR, configData, sizeof(float)*4);
mborchers 0:8a6003b8bb5b 120
mborchers 0:8a6003b8bb5b 121 // Flag setzen
mborchers 0:8a6003b8bb5b 122 uint8_t command = 1<<3;
mborchers 0:8a6003b8bb5b 123 supportSystem->writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_COMMAND, &command, sizeof(uint8_t));
mborchers 0:8a6003b8bb5b 124
mborchers 0:8a6003b8bb5b 125 bool timer_started = false;
mborchers 0:8a6003b8bb5b 126
mborchers 0:8a6003b8bb5b 127 wait(0.1);
mborchers 0:8a6003b8bb5b 128
mborchers 2:bf739d7d9f8f 129 steering_angle_minnow_queue = 0.0;
mborchers 2:bf739d7d9f8f 130 quadrature_queue.put(&steering_angle_minnow_queue);
mborchers 2:bf739d7d9f8f 131
mborchers 2:bf739d7d9f8f 132 velocity_minnow_queue = 1.5;
mborchers 2:bf739d7d9f8f 133 machine_direction_queue.put(&velocity_minnow_queue);
mborchers 0:8a6003b8bb5b 134
mborchers 0:8a6003b8bb5b 135 while(true) {
mborchers 0:8a6003b8bb5b 136 IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer();
mborchers 0:8a6003b8bb5b 137 RadioDecoder_registerDataBuffer = supportSystem->getRadioDecoderRegisterDataBuffer();
mborchers 0:8a6003b8bb5b 138
mborchers 0:8a6003b8bb5b 139 for(uint8_t i=0; i<3; i++) {
mborchers 0:8a6003b8bb5b 140 serialMinnow.printf("RadioDecoder - Ch[%d] Valid: %d\r\n",i,RadioDecoder_registerDataBuffer->channelValid[i]);
mborchers 0:8a6003b8bb5b 141 serialMinnow.printf("RadioDecoder - Ch[%d] ActiveTime: %d\r\n",i,RadioDecoder_registerDataBuffer->channelActiveTime[i]);
mborchers 0:8a6003b8bb5b 142 serialMinnow.printf("RadioDecoder - Ch[%d] Percentage: %d\r\n",i,RadioDecoder_registerDataBuffer->channelPercent[i]);
mborchers 0:8a6003b8bb5b 143 }
mborchers 0:8a6003b8bb5b 144
mborchers 0:8a6003b8bb5b 145 uint16_t rc_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[0];
mborchers 0:8a6003b8bb5b 146 uint8_t rc_valid = RadioDecoder_registerDataBuffer->channelValid[0];
mborchers 0:8a6003b8bb5b 147
mborchers 0:8a6003b8bb5b 148 uint16_t drive_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[1];
mborchers 0:8a6003b8bb5b 149 uint8_t drive_valid = RadioDecoder_registerDataBuffer->channelValid[1];
mborchers 0:8a6003b8bb5b 150
mborchers 0:8a6003b8bb5b 151 uint16_t steer_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[2];
mborchers 0:8a6003b8bb5b 152 uint8_t steer_valid = RadioDecoder_registerDataBuffer->channelValid[2];
mborchers 0:8a6003b8bb5b 153
mborchers 0:8a6003b8bb5b 154
mborchers 0:8a6003b8bb5b 155 if (rc_percentage > (uint16_t) 1800 && rc_valid != 0) {
mborchers 0:8a6003b8bb5b 156 // oben => Wettbewerb
mborchers 0:8a6003b8bb5b 157 heartbeatLED = true;
mborchers 0:8a6003b8bb5b 158 buttonLED = false;
mborchers 0:8a6003b8bb5b 159 redlightLED = false;
mborchers 0:8a6003b8bb5b 160 supportSystem->setLightManagerRemoteLight(false, true);
mborchers 0:8a6003b8bb5b 161 if (!timer_started) {
mborchers 0:8a6003b8bb5b 162 timer_started = true;
mborchers 0:8a6003b8bb5b 163 machine_direction_control_timer.start(10);
mborchers 0:8a6003b8bb5b 164 quadrature_control_timer.start(10);
mborchers 0:8a6003b8bb5b 165 }
mborchers 0:8a6003b8bb5b 166 } else if (rc_percentage > (uint16_t) 1200 && rc_valid != 0) {
mborchers 0:8a6003b8bb5b 167 // unten => RC-Wettbewerb
mborchers 0:8a6003b8bb5b 168 heartbeatLED = false;
mborchers 0:8a6003b8bb5b 169 buttonLED = false;
mborchers 0:8a6003b8bb5b 170 redlightLED = true;
mborchers 0:8a6003b8bb5b 171 supportSystem->setLightManagerRemoteLight(true, true);
mborchers 0:8a6003b8bb5b 172 if (drive_valid) {
mborchers 0:8a6003b8bb5b 173 drivePWM.pulsewidth_us(drive_percentage);
mborchers 0:8a6003b8bb5b 174 }
mborchers 0:8a6003b8bb5b 175 if (steer_valid) {
mborchers 0:8a6003b8bb5b 176 steerPWM.pulsewidth_us(steer_percentage);
mborchers 0:8a6003b8bb5b 177 }
mborchers 0:8a6003b8bb5b 178 if (timer_started) {
mborchers 0:8a6003b8bb5b 179 timer_started = false;
mborchers 0:8a6003b8bb5b 180 machine_direction_control_timer.stop();
mborchers 0:8a6003b8bb5b 181 quadrature_control_timer.stop();
mborchers 0:8a6003b8bb5b 182 }
mborchers 0:8a6003b8bb5b 183 } else if (rc_percentage > (uint16_t) 800 && rc_valid != 0) {
mborchers 0:8a6003b8bb5b 184 // mitte => RC-Training
mborchers 0:8a6003b8bb5b 185 heartbeatLED = false;
mborchers 0:8a6003b8bb5b 186 buttonLED = true;
mborchers 0:8a6003b8bb5b 187 redlightLED = false;
mborchers 0:8a6003b8bb5b 188 supportSystem->setLightManagerRemoteLight(true, true);
mborchers 0:8a6003b8bb5b 189 if (drive_valid) {
mborchers 0:8a6003b8bb5b 190 drivePWM.pulsewidth_us(drive_percentage);
mborchers 0:8a6003b8bb5b 191 }
mborchers 0:8a6003b8bb5b 192 if (steer_valid) {
mborchers 0:8a6003b8bb5b 193 steerPWM.pulsewidth_us(steer_percentage);
mborchers 0:8a6003b8bb5b 194 }
mborchers 0:8a6003b8bb5b 195 if (timer_started) {
mborchers 0:8a6003b8bb5b 196 timer_started = false;
mborchers 0:8a6003b8bb5b 197 machine_direction_control_timer.stop();
mborchers 0:8a6003b8bb5b 198 quadrature_control_timer.stop();
mborchers 0:8a6003b8bb5b 199 }
mborchers 0:8a6003b8bb5b 200 }
mborchers 0:8a6003b8bb5b 201
mborchers 0:8a6003b8bb5b 202 velocity_imu_queue = IMU_registerDataBuffer->velocityXFilteredRegister;
mborchers 0:8a6003b8bb5b 203 imu_queue_velocity.put(&velocity_imu_queue);
mborchers 0:8a6003b8bb5b 204
mborchers 0:8a6003b8bb5b 205 float radius = IMU_registerDataBuffer->velocityXFilteredRegister/IMU_registerDataBuffer->velocityAngularFilteredRegister;
mborchers 0:8a6003b8bb5b 206
mborchers 0:8a6003b8bb5b 207 steering_angle_imu_queue = atan(0.205/radius)*180/PI;
mborchers 0:8a6003b8bb5b 208 imu_queue_steering_angle.put(&steering_angle_imu_queue);
mborchers 0:8a6003b8bb5b 209
mborchers 0:8a6003b8bb5b 210
mborchers 2:bf739d7d9f8f 211 //serialMinnow.printf("%ld, ", difference_millis);
mborchers 2:bf739d7d9f8f 212 serialMinnow.printf("%f, ", velocity_minnow_queue);
mborchers 2:bf739d7d9f8f 213 serialMinnow.printf("%f, ", steering_angle_minnow_queue);
mborchers 2:bf739d7d9f8f 214 serialMinnow.printf("%d, ", IMU_registerDataBuffer->curveRadiusRegister);
mborchers 2:bf739d7d9f8f 215 serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityAngularRegister);
mborchers 2:bf739d7d9f8f 216 serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityAngularFilteredRegister);
mborchers 2:bf739d7d9f8f 217 serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityXRegister);
mborchers 2:bf739d7d9f8f 218 serialMinnow.printf("%f\r\n", IMU_registerDataBuffer->velocityXFilteredRegister);
mborchers 0:8a6003b8bb5b 219 Thread::wait(50);
mborchers 0:8a6003b8bb5b 220 }
mborchers 0:8a6003b8bb5b 221 }