Mechatronics / Mbed 2 deprecated FRDM_wahadlo_torsyjne

FRDM_wahadlo_torsyjne

Dependencies:   mbed BufferedSerial

Fork of FRDM_wahadlo_torsyjne by Wojciech M

main.cpp@19:2012df6b8e56, 2019-02-27 (annotated)

Committer:
Lukasz_K
Date:
Wed Feb 27 21:13:54 2019 +0000
Revision:
19:2012df6b8e56
Parent:
18:97a08a408bc7 
termistor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wojtekmir 13:8810ad3ccc04 1 //Program zmodyfikowany. Instrukcja obsługi:
wojtekmir 13:8810ad3ccc04 2 //Po włączeniu zasilania sterownika SMC (26V) i podłączeniu przewodu USB silnik zostaje zablokowany (ciężko go ruszyć)
wojtekmir 13:8810ad3ccc04 3 // w celu ustawienia punktu początkowego trzymamy przycisk RESET na płytce FRDM (silnik obraca się łatwo)
wojtekmir 13:8810ad3ccc04 4 // po ustawieniu punktu początkowego puszczamy przycisk RESET
wojtekmir 13:8810ad3ccc04 5 // sterownik inicjalizuje program, zapisuje położenia zerowe czujników
wojtekmir 13:8810ad3ccc04 6 // po odliczeniu 5 sekund wykonuje procedurę startową (przekręca silnik o połowę ustawionego zakresu i
wojtekmir 13:8810ad3ccc04 7 // zmienia stan wyjścia direction
wojtekmir 13:8810ad3ccc04 8 // następnie wykonywane są przerwania pomiaru i sterowania
wojtekmir 13:8810ad3ccc04 9 //
wojtekmir 13:8810ad3ccc04 10 // na oscyloskopie widać, że wysyłanie danych pomiarowych nie wpływa na sterowanie silnikiem (wyjście direction) i sygnał PWM
wojtekmir 13:8810ad3ccc04 11 //
wojtekmir 13:8810ad3ccc04 12 // dołączenie dodatkowego kanału dla górnego czujnika nic nieda, ponieważ sygnał jest odwrócony w fazie o 180 stopni,
wojtekmir 13:8810ad3ccc04 13 // a nie przesunięty w fazie- nie da się na jego podstawie ustlić kierunku obrotu ani osiągnięcia pełnego obrotu
wojtekmir 13:8810ad3ccc04 14 // ciała swobodnego
wojtekmir 13:8810ad3ccc04 15
wojtekmir 1:a4f9276a0f35 16 #include "mbed.h"
Pawel_13 18:97a08a408bc7 17 #include "BufferedSerial.h"
Lukasz_K 19:2012df6b8e56 18 #include "math.h"
Lukasz_K 19:2012df6b8e56 19
Lukasz_K 19:2012df6b8e56 20 //=========TERMISTOR==================
Lukasz_K 19:2012df6b8e56 21 #define beta 3950 // stała materiałowa termistora [K]
Lukasz_K 19:2012df6b8e56 22 #define T0 298 // temperatura odniesienia (zwykle 298K) [K]
Lukasz_K 19:2012df6b8e56 23 #define R0 10000 // rezystancja termistora w temperaturze odniesienia [Ω]
Lukasz_K 19:2012df6b8e56 24 #define R 16700 // rezystancja opornika w dzielniku [Ω]
Lukasz_K 19:2012df6b8e56 25
wojtekmir 1:a4f9276a0f35 26
Pawel_13 18:97a08a408bc7 27 Ticker triger1; //interrupt. No. 1
Pawel_13 18:97a08a408bc7 28 Ticker triger2; //interrupt No. 2
wojtekmir 10:497e9a0da7d6 29
Pawel_13 18:97a08a408bc7 30 int motor_PWM_period_us = 500; //period of PWM signal in mikroseconds
Pawel_13 18:97a08a408bc7 31 // 3200 kroków to pełny kąt
Pawel_13 18:97a08a408bc7 32 int EXC_angle = 1600/2/4; //360 stopni to 3200 krok. przy mkrok. = 1.8/16 dzielic przez 2
Pawel_13 18:97a08a408bc7 33 //float switch_dir_time=(float)((float)EXC_angle/500.0)*((float)motor_PWM_period_us/1000.0);
wojtekmir 11:b3bd4b86f914 34 float switch_dir_time=(float)((float)EXC_angle/500.0)*((float)motor_PWM_period_us/1000.0);
Pawel_13 18:97a08a408bc7 35 //float switch_dir_time = 1;
Lukasz_K 19:2012df6b8e56 36 float sampling_time = 0.2;//0.005
Pawel_13 18:97a08a408bc7 37 int samples_in_period = int(switch_dir_time/sampling_time);
wojtekmir 12:103f3fbeab78 38 int zero_analog_IN_0 = 0;
wojtekmir 12:103f3fbeab78 39 int zero_analog_IN_1 = 0;
Lukasz_K 19:2012df6b8e56 40 int zero_analog_IN_2 = 0;
Lukasz_K 19:2012df6b8e56 41 int zero_analog_IN_3 = 0;
Lukasz_K 19:2012df6b8e56 42 int zero_analog_IN_4 = 0;
Lukasz_K 19:2012df6b8e56 43 int zero_analog_IN_5 = 0;
Pawel_13 18:97a08a408bc7 44 float timet = 0;
Lukasz_K 19:2012df6b8e56 45
Lukasz_K 19:2012df6b8e56 46 //volatile float t;
wojtekmir 12:103f3fbeab78 47
wojtekmir 13:8810ad3ccc04 48 DigitalOut direction(PTC7); // zmiana wartości logicznej na wyjściu PTC7 powoduje zmianę kieruneku obrotów
Pawel_13 18:97a08a408bc7 49 DigitalOut enable_driver(PTE2); // zezwolenie na start
wojtekmir 13:8810ad3ccc04 50 DigitalOut myled(LED_GREEN); // dioda zielona sygnalizuje zmianę kierunku obrotów
Lukasz_K 19:2012df6b8e56 51 AnalogIn analog_IN_0(PTB0); //PTB0 // odczyt napięcia z wejścia analogowego 0 (kąt obrotu kolumny)
Lukasz_K 19:2012df6b8e56 52 AnalogIn analog_IN_1(PTB1); //PTB1
Lukasz_K 19:2012df6b8e56 53 AnalogIn analog_IN_2(PTB2); //PTB2 // PTB1 - sygnał odwrócony
Lukasz_K 19:2012df6b8e56 54 AnalogIn analog_IN_3(PTB3); //PTB3
Lukasz_K 19:2012df6b8e56 55 AnalogIn analog_IN_4(PTC2); //PTC2
Lukasz_K 19:2012df6b8e56 56 AnalogIn analog_IN_5(PTC1); //PTC1 // odczyt napięcia z wejścia analogowego 1 (kąt obrotu ciała swobodnego)
Pawel_13 18:97a08a408bc7 57 // PTB3 - sygnał odwrócony
wojtekmir 13:8810ad3ccc04 58 PwmOut motor(PTA5); // sygnał PWM do sterowania sterownikiem silnika krokowego
Pawel_13 18:97a08a408bc7 59 //Serial pc(USBTX,USBRX); // komunikacja z PC
Pawel_13 18:97a08a408bc7 60 BufferedSerial pc(USBTX,USBRX);
Pawel_13 18:97a08a408bc7 61 char buffer[24];
Pawel_13 9:3540ad710232 62
Pawel_13 8:434d613f0929 63 float get_voltage_in(AnalogIn analog_in, float zero_analog_in) {
wojtekmir 13:8810ad3ccc04 64 return (analog_in.read() - zero_analog_in) * 2.9035; // przesunięcie do zera i skalowanie napiecia do zakresu
Pawel_13 3:2ba53b9a499a 65 }
Lukasz_K 19:2012df6b8e56 66
Lukasz_K 19:2012df6b8e56 67 float temperatura_K(AnalogIn analog){
Lukasz_K 19:2012df6b8e56 68 float t=1.65-analog; //1,65 połowa zakresu VREF
Lukasz_K 19:2012df6b8e56 69 t= t/analog;
Lukasz_K 19:2012df6b8e56 70 t= log(t);
Lukasz_K 19:2012df6b8e56 71 t=t + beta/T0+ 0,23; //0,23 - log R/R0
Lukasz_K 19:2012df6b8e56 72 t=beta/t;
Lukasz_K 19:2012df6b8e56 73 return t;
Lukasz_K 19:2012df6b8e56 74 }
Lukasz_K 19:2012df6b8e56 75
Lukasz_K 19:2012df6b8e56 76
wojtekmir 10:497e9a0da7d6 77 void task1() {
wojtekmir 10:497e9a0da7d6 78 myled = !myled;
wojtekmir 10:497e9a0da7d6 79 direction = !direction;
Pawel_13 18:97a08a408bc7 80 //pc.printf("%.2f\t%d\n\r", timet, int(direction));
wojtekmir 10:497e9a0da7d6 81 }
Pawel_13 3:2ba53b9a499a 82
wojtekmir 10:497e9a0da7d6 83 void task2() {
Pawel_13 18:97a08a408bc7 84 //get_voltage_in(analog_IN_0, zero_analog_IN_0);
Pawel_13 18:97a08a408bc7 85 //get_voltage_in(analog_IN_1, zero_analog_IN_1);
Pawel_13 18:97a08a408bc7 86 //pc.printf("%.2f\t%.2f\t%.2f\n\r", timet, get_voltage_in(analog_IN_1, zero_analog_IN_1), get_voltage_in(analog_IN_0, zero_analog_IN_0));
Lukasz_K 19:2012df6b8e56 87 //pc.printf("%.2f %.2f %.2f\n\r", timet, get_voltage_in(analog_IN_1, zero_analog_IN_1), get_voltage_in(analog_IN_0, zero_analog_IN_0));
Pawel_13 18:97a08a408bc7 88
Lukasz_K 19:2012df6b8e56 89 sprintf((char*)buffer, "%.2f %.2f %.2f %.2f %.2f %.2f\n\r",get_voltage_in(analog_IN_5, zero_analog_IN_5), get_voltage_in(analog_IN_4, zero_analog_IN_4),
Lukasz_K 19:2012df6b8e56 90 get_voltage_in(analog_IN_3, zero_analog_IN_3), get_voltage_in(analog_IN_2, zero_analog_IN_2),
Lukasz_K 19:2012df6b8e56 91 get_voltage_in(analog_IN_1, zero_analog_IN_1), get_voltage_in(analog_IN_0, zero_analog_IN_0));
Pawel_13 18:97a08a408bc7 92 pc.printf(buffer);
Pawel_13 18:97a08a408bc7 93
Lukasz_K 19:2012df6b8e56 94 //sprintf((char*)buffer, " %.2f %.2f\n\r",temperatura_K(analog_IN_1), get_voltage_in(analog_IN_0, zero_analog_IN_0));
Lukasz_K 19:2012df6b8e56 95 //pc.printf(buffer);
Lukasz_K 19:2012df6b8e56 96
Lukasz_K 19:2012df6b8e56 97 //pc.printf("%.2f", t);
Lukasz_K 19:2012df6b8e56 98
Pawel_13 18:97a08a408bc7 99 timet += sampling_time;
Pawel_13 18:97a08a408bc7 100 }
Pawel_13 8:434d613f0929 101
Pawel_13 8:434d613f0929 102 int main() {
wojtekmir 13:8810ad3ccc04 103 pc.baud(115200); // największa szybkość standardowa, jaką można zadać to 19200
wojtekmir 13:8810ad3ccc04 104 // im większa prędkość transmisji tym mniejsze czasy wysyłania- więcej niż 115200 nie osiągalne
wojtekmir 13:8810ad3ccc04 105 // dla 115200 mamy mniejsze czasy wysyłania (możemy wysyłać częściej- co 10ms)
Pawel_13 18:97a08a408bc7 106 enable_driver = 1; // enable signal to allow the driver for work
wojtekmir 12:103f3fbeab78 107 zero_analog_IN_0 = analog_IN_0.read();
Pawel_13 18:97a08a408bc7 108 zero_analog_IN_1 = analog_IN_1.read();
Pawel_13 18:97a08a408bc7 109 //pc.printf("Switch direction time: %.6f\n\r", switch_dir_time);
Pawel_13 18:97a08a408bc7 110 //pc.printf("Samples in period: %d\n\r", samples_in_period);
Pawel_13 18:97a08a408bc7 111 //pc.printf("Sampling time: %.6f\n\r", sampling_time);
Pawel_13 18:97a08a408bc7 112 wait(6); //czas na ustawienia początkowe 10 sek.
wojtekmir 13:8810ad3ccc04 113 motor.period_us(motor_PWM_period_us);
Pawel_13 18:97a08a408bc7 114 motor = 0.5; // duty cycle of the PWM signal
Pawel_13 18:97a08a408bc7 115 timet = 0.0;
Pawel_13 18:97a08a408bc7 116 //wait(switch_dir_time/2); // położenie zerowe silnika do ustalenia zer czujników
Pawel_13 18:97a08a408bc7 117 //wait_ms(1800);
wojtekmir 13:8810ad3ccc04 118 direction = !direction; // i uruchomienie silnika
Pawel_13 18:97a08a408bc7 119 switch_dir_time = 0.9;
Pawel_13 18:97a08a408bc7 120 triger2.attach(&task2, sampling_time);
wojtekmir 11:b3bd4b86f914 121 triger1.attach(&task1, switch_dir_time);
wojtekmir 10:497e9a0da7d6 122
Pawel_13 8:434d613f0929 123 while (true) {
Pawel_13 18:97a08a408bc7 124 wait(10); // zajęcie sterownika, zabezpieczenie przed zawieszeniem pracy sterownika
wojtekmir 1:a4f9276a0f35 125 }
wojtekmir 11:b3bd4b86f914 126 }