Zeran Gu / Mbed 2 deprecated STM32F103C8T6_coaxial_controlMixer

still working in progress

Dependencies:   mbed-STM32F103C8T6 mbed

Fork of STM32F103C8T6_Hello by Zoltan Hudak

main.cpp@11:69a364e391dc, 2017-05-31 (annotated)

Committer:
Zeran
Date:
Wed May 31 13:57:55 2017 +0000
Revision:
11:69a364e391dc
Parent:
10:4b88be251088 
mixer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hudakz 5:3c3ef17a17a6 1 #include "stm32f103c8t6.h"
hudakz 0:ab218237069e 2 #include "mbed.h"
Zeran 11:69a364e391dc 3 I2C i2c(PB_9, PB_8);
Zeran 11:69a364e391dc 4 const int address = 0x08 << 1;
Zeran 11:69a364e391dc 5 Timer t;
Zeran 11:69a364e391dc 6 int Now = 0 , LastUpdate = 0, deltat = 0;
Zeran 11:69a364e391dc 7 int main()
Zeran 11:69a364e391dc 8 {
hudakz 10:4b88be251088 9 confSysClock(); //Configure system clock (72MHz HSE clock, 48MHz USB clock)
Zeran 11:69a364e391dc 10
hudakz 10:4b88be251088 11 Serial pc(PA_2, PA_3);
Zeran 11:69a364e391dc 12 pc.baud(115200);
Zeran 11:69a364e391dc 13 //AnalogIn nub1(PA_0);
Zeran 11:69a364e391dc 14 PwmOut servo1(PA_6);
Zeran 11:69a364e391dc 15 PwmOut servo2(PA_7);
Zeran 11:69a364e391dc 16 PwmOut servo3(PB_0);
Zeran 11:69a364e391dc 17 PwmOut servo4(PB_1);
Zeran 11:69a364e391dc 18 servo1.period_us(20000);
Zeran 11:69a364e391dc 19 servo2.period_us(20000);
Zeran 11:69a364e391dc 20 servo3.period_us(20000);
Zeran 11:69a364e391dc 21 servo4.period_us(20000);
Zeran 11:69a364e391dc 22
Zeran 11:69a364e391dc 23 float pitchrig = 0.0164;//41*8/20000;
Zeran 11:69a364e391dc 24 float pitchlft = 0.0276;//69*8/20000;
Zeran 11:69a364e391dc 25 float pitchout = 0.0224;//56*8/20000;
Zeran 11:69a364e391dc 26
Zeran 11:69a364e391dc 27
Zeran 11:69a364e391dc 28 float rollrig = 0.014;//35
Zeran 11:69a364e391dc 29 float rolllft = 0.0308;//77
Zeran 11:69a364e391dc 30 float rollout = 0.0224;//56
Zeran 11:69a364e391dc 31
Zeran 11:69a364e391dc 32 float Motr_up = 0.0448;//112
Zeran 11:69a364e391dc 33 float MotorA = 0;
Zeran 11:69a364e391dc 34 float MotorB = 0;
Zeran 11:69a364e391dc 35
Zeran 11:69a364e391dc 36 float MotorAi = 0;
Zeran 11:69a364e391dc 37 // float MotorBi = 0;
Zeran 11:69a364e391dc 38
Zeran 11:69a364e391dc 39 float diff = 0;
Zeran 11:69a364e391dc 40
Zeran 11:69a364e391dc 41 servo1 = 0.05f + pitchout;
Zeran 11:69a364e391dc 42 servo2 = 0.05f + rollout;
Zeran 11:69a364e391dc 43 servo3 = 0.05f + MotorA;
Zeran 11:69a364e391dc 44 servo4 = 0.05f + MotorB;
Zeran 11:69a364e391dc 45 t.start();
Zeran 11:69a364e391dc 46
hudakz 0:ab218237069e 47 while(1) {
Zeran 11:69a364e391dc 48 char data[5];
Zeran 11:69a364e391dc 49 i2c.read(address,data,5);
Zeran 11:69a364e391dc 50 //pc.printf("%d,%d,%d,%d,%d\r\n", data[0], data[1], data[2], data[3], data[4]);
Zeran 11:69a364e391dc 51 // if (pc.readable()) {
Zeran 11:69a364e391dc 52 // char channel = pc.getc();
Zeran 11:69a364e391dc 53 // if (channel>128) {
Zeran 11:69a364e391dc 54 // char control = pc.getc();
Zeran 11:69a364e391dc 55 // if (channel == 129 && control<=128) {
Zeran 11:69a364e391dc 56 Now = t.read_us();
Zeran 11:69a364e391dc 57 deltat = Now - LastUpdate; // set integration time by time elapsed since last filter update
Zeran 11:69a364e391dc 58 LastUpdate = Now;
Zeran 11:69a364e391dc 59
Zeran 11:69a364e391dc 60 pitchout = float(data[0]*4)/20000;
Zeran 11:69a364e391dc 61
Zeran 11:69a364e391dc 62 if(pitchout>pitchlft) {
Zeran 11:69a364e391dc 63 pitchout = pitchlft;
Zeran 11:69a364e391dc 64 }
Zeran 11:69a364e391dc 65 if(pitchout<pitchrig) {
Zeran 11:69a364e391dc 66 pitchout = pitchrig;
Zeran 11:69a364e391dc 67 }
Zeran 11:69a364e391dc 68 if(deltat > 4000){
Zeran 11:69a364e391dc 69 servo1 = 0.05f + pitchout;
Zeran 11:69a364e391dc 70 }
Zeran 11:69a364e391dc 71
Zeran 11:69a364e391dc 72 // }
Zeran 11:69a364e391dc 73 // if (channel == 130 && control<=128) {
Zeran 11:69a364e391dc 74 rollout = float(data[1]*4)/20000;
Zeran 11:69a364e391dc 75 //pc.printf("rollout = %f\n\r", rollout);
Zeran 11:69a364e391dc 76 if(rollout>rolllft) {
Zeran 11:69a364e391dc 77 rollout = rolllft;
Zeran 11:69a364e391dc 78 }
Zeran 11:69a364e391dc 79 if(rollout<rollrig) {
Zeran 11:69a364e391dc 80 rollout = rollrig;
Zeran 11:69a364e391dc 81 }
Zeran 11:69a364e391dc 82 //servo2 = 0.05f + rollout;
Zeran 11:69a364e391dc 83 //servo2 = 0.05f+float(control*8)/20000;
Zeran 11:69a364e391dc 84
Zeran 11:69a364e391dc 85 // }
Zeran 11:69a364e391dc 86 // if (channel == 131 && control<=128) {
Zeran 11:69a364e391dc 87 MotorAi = float(data[2]*4)/20000;
Zeran 11:69a364e391dc 88 //pc.printf("MotorAi = %f\n\r", MotorAi);
Zeran 11:69a364e391dc 89 diff = float((int16_t(data[3]/2)-64)*0.0004);
Zeran 11:69a364e391dc 90 pc.printf("%f,%f,%f,%f\n\r", pitchout,rollout,MotorAi,diff);
Zeran 11:69a364e391dc 91 MotorA = MotorAi+diff;
Zeran 11:69a364e391dc 92 if(MotorA>Motr_up) {
Zeran 11:69a364e391dc 93 MotorA = Motr_up;
Zeran 11:69a364e391dc 94 }
Zeran 11:69a364e391dc 95 if(MotorA<0) {
Zeran 11:69a364e391dc 96 MotorA = 0;
Zeran 11:69a364e391dc 97 }
Zeran 11:69a364e391dc 98
Zeran 11:69a364e391dc 99 MotorB = MotorAi - diff;
Zeran 11:69a364e391dc 100 if(MotorB>Motr_up) {
Zeran 11:69a364e391dc 101 MotorB = Motr_up;
Zeran 11:69a364e391dc 102 }
Zeran 11:69a364e391dc 103 if(MotorB<0) {
Zeran 11:69a364e391dc 104 MotorB = 0;
Zeran 11:69a364e391dc 105 }
Zeran 11:69a364e391dc 106 //servo3 = 0.05f + MotorA;
Zeran 11:69a364e391dc 107 //servo4 = 0.05f + MotorB;
Zeran 11:69a364e391dc 108 //servo2 = 0.05f+float(control*8)/20000;
Zeran 11:69a364e391dc 109
Zeran 11:69a364e391dc 110 // }
Zeran 11:69a364e391dc 111 /* if (channel == 132 && control<=128) {
Zeran 11:69a364e391dc 112 diff = float((int16_t(control)-64)*0.0004);
Zeran 11:69a364e391dc 113 //pc.printf("diff = %f\n\r", diff);
Zeran 11:69a364e391dc 114
Zeran 11:69a364e391dc 115 MotorA = MotorAi+diff;
Zeran 11:69a364e391dc 116 if(MotorA>Motr_up) {
Zeran 11:69a364e391dc 117 MotorA = Motr_up;
Zeran 11:69a364e391dc 118 }
Zeran 11:69a364e391dc 119 if(MotorA<0) {
Zeran 11:69a364e391dc 120 MotorA = 0;
Zeran 11:69a364e391dc 121 }
Zeran 11:69a364e391dc 122
Zeran 11:69a364e391dc 123 MotorB = MotorAi-diff;
Zeran 11:69a364e391dc 124 if(MotorB>Motr_up) {
Zeran 11:69a364e391dc 125 MotorB = Motr_up;
Zeran 11:69a364e391dc 126 }
Zeran 11:69a364e391dc 127 if(MotorB<0) {
Zeran 11:69a364e391dc 128 MotorB = 0;
Zeran 11:69a364e391dc 129 }
Zeran 11:69a364e391dc 130 servo3 = 0.05f + MotorA;
Zeran 11:69a364e391dc 131 servo4 = 0.05f + MotorB;
Zeran 11:69a364e391dc 132 }
Zeran 11:69a364e391dc 133 }*/
Zeran 11:69a364e391dc 134 //servo = float(1000+(c*4))/20000;
Zeran 11:69a364e391dc 135 //}
hudakz 0:ab218237069e 136 }
hudakz 0:ab218237069e 137 }
Zeran 11:69a364e391dc 138
Zeran 11:69a364e391dc 139
Zeran 11:69a364e391dc 140
Zeran 11:69a364e391dc 141
Zeran 11:69a364e391dc 142
Zeran 11:69a364e391dc 143
Zeran 11:69a364e391dc 144
Zeran 11:69a364e391dc 145