![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Triangular Omni-wheels
Dependencies: mbed Test2Boards
main.cpp@3:4b7a8404c42d, 2021-11-27 (annotated)
- Committer:
- ea78anana
- Date:
- Sat Nov 27 14:44:11 2021 +0000
- Revision:
- 3:4b7a8404c42d
- Parent:
- 2:c2106a1bce04
- Child:
- 4:013f9a62dec8
kkk
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kelhon30 | 0:f5797bc73f93 | 1 | #include "mbed.h" |
kelhon30 | 1:cb2586b26e9b | 2 | #include "rtos/rtos.h" |
kelhon30 | 1:cb2586b26e9b | 3 | #include "Teseo-LIV3F.h" |
kelhon30 | 1:cb2586b26e9b | 4 | #include "XNucleoIKS01A2.h" |
ea78anana | 2:c2106a1bce04 | 5 | |
kelhon30 | 0:f5797bc73f93 | 6 | |
kelhon30 | 0:f5797bc73f93 | 7 | #define Awheel_A D2 //A phase |
kelhon30 | 0:f5797bc73f93 | 8 | #define Awheel_B D3 //B phase |
kelhon30 | 0:f5797bc73f93 | 9 | #define Awheel_Z D4 //Z phase |
kelhon30 | 0:f5797bc73f93 | 10 | #define Bwheel_A D6 //A phase |
kelhon30 | 0:f5797bc73f93 | 11 | #define Bwheel_B D7 //B phase |
kelhon30 | 0:f5797bc73f93 | 12 | #define Bwheel_Z D8 //Z phase |
ea78anana | 3:4b7a8404c42d | 13 | #define Cwheel_A D14 //A phase |
ea78anana | 3:4b7a8404c42d | 14 | #define Cwheel_B D15 //B phase |
kelhon30 | 0:f5797bc73f93 | 15 | #define Cwheel_Z D12 //Z phase |
kelhon30 | 1:cb2586b26e9b | 16 | |
kelhon30 | 0:f5797bc73f93 | 17 | |
kelhon30 | 0:f5797bc73f93 | 18 | #define time2 10000 |
kelhon30 | 0:f5797bc73f93 | 19 | #define HIGH 1 |
kelhon30 | 0:f5797bc73f93 | 20 | #define LOW 0 |
kelhon30 | 0:f5797bc73f93 | 21 | |
kelhon30 | 1:cb2586b26e9b | 22 | Thread threadA, threadB, threadC; |
kelhon30 | 0:f5797bc73f93 | 23 | |
kelhon30 | 0:f5797bc73f93 | 24 | Serial pc(USBTX, USBRX); |
kelhon30 | 0:f5797bc73f93 | 25 | |
kelhon30 | 0:f5797bc73f93 | 26 | //Initialize Variable |
ea78anana | 3:4b7a8404c42d | 27 | const float d = 5.8; //Diameter of the wheel |
kelhon30 | 0:f5797bc73f93 | 28 | const float pi = 3.141592654;//PI |
kelhon30 | 0:f5797bc73f93 | 29 | |
kelhon30 | 0:f5797bc73f93 | 30 | //A wheel Variable |
kelhon30 | 0:f5797bc73f93 | 31 | int Acounter_cw = 0; |
kelhon30 | 0:f5797bc73f93 | 32 | int Acounter_ccw = 0; |
kelhon30 | 0:f5797bc73f93 | 33 | int Anum = 0;//number of turns |
kelhon30 | 0:f5797bc73f93 | 34 | double At;//time per turn |
kelhon30 | 0:f5797bc73f93 | 35 | float Avelocity; |
kelhon30 | 0:f5797bc73f93 | 36 | int Acurrent = 0; |
ea78anana | 3:4b7a8404c42d | 37 | float Atemp = 0; |
kelhon30 | 0:f5797bc73f93 | 38 | int An = 0; |
ea78anana | 3:4b7a8404c42d | 39 | float Adistance = 0; |
kelhon30 | 0:f5797bc73f93 | 40 | double Atime3;//Time of phase Z detected, use for calculate the velocity |
kelhon30 | 0:f5797bc73f93 | 41 | Timer Af; |
kelhon30 | 0:f5797bc73f93 | 42 | |
kelhon30 | 0:f5797bc73f93 | 43 | DigitalIn a12(Awheel_B); |
kelhon30 | 0:f5797bc73f93 | 44 | InterruptIn a11(Awheel_A); |
kelhon30 | 0:f5797bc73f93 | 45 | InterruptIn a13(Awheel_Z); |
kelhon30 | 0:f5797bc73f93 | 46 | |
kelhon30 | 0:f5797bc73f93 | 47 | void EncodeA() |
kelhon30 | 0:f5797bc73f93 | 48 | { |
kelhon30 | 0:f5797bc73f93 | 49 | if((a11 == HIGH) && (a12 == LOW)) |
kelhon30 | 1:cb2586b26e9b | 50 | |
kelhon30 | 0:f5797bc73f93 | 51 | { Acounter_cw++; |
kelhon30 | 0:f5797bc73f93 | 52 | } |
kelhon30 | 1:cb2586b26e9b | 53 | |
ea78anana | 2:c2106a1bce04 | 54 | else |
kelhon30 | 1:cb2586b26e9b | 55 | { Acounter_cw--; |
ea78anana | 2:c2106a1bce04 | 56 | |
kelhon30 | 1:cb2586b26e9b | 57 | } |
kelhon30 | 1:cb2586b26e9b | 58 | |
kelhon30 | 0:f5797bc73f93 | 59 | } |
kelhon30 | 0:f5797bc73f93 | 60 | |
kelhon30 | 0:f5797bc73f93 | 61 | void Asetup(){ |
kelhon30 | 0:f5797bc73f93 | 62 | a11.mode(PullUp); |
kelhon30 | 0:f5797bc73f93 | 63 | a12.mode(PullUp); |
kelhon30 | 0:f5797bc73f93 | 64 | a11.rise(&EncodeA); |
kelhon30 | 0:f5797bc73f93 | 65 | } |
kelhon30 | 1:cb2586b26e9b | 66 | |
ea78anana | 2:c2106a1bce04 | 67 | |
kelhon30 | 1:cb2586b26e9b | 68 | void ASet_state(int a){ |
kelhon30 | 1:cb2586b26e9b | 69 | Acounter_cw = a; |
kelhon30 | 0:f5797bc73f93 | 70 | An = 0; |
kelhon30 | 0:f5797bc73f93 | 71 | } |
kelhon30 | 0:f5797bc73f93 | 72 | |
ea78anana | 2:c2106a1bce04 | 73 | |
kelhon30 | 0:f5797bc73f93 | 74 | void Aloop() |
kelhon30 | 1:cb2586b26e9b | 75 | |
kelhon30 | 0:f5797bc73f93 | 76 | { |
kelhon30 | 0:f5797bc73f93 | 77 | //clockwise turning |
ea78anana | 3:4b7a8404c42d | 78 | An = An + 1; |
ea78anana | 3:4b7a8404c42d | 79 | if (Acounter_cw >= 0) |
ea78anana | 3:4b7a8404c42d | 80 | { |
ea78anana | 3:4b7a8404c42d | 81 | Atemp = Acounter_cw / 2500.0; |
ea78anana | 3:4b7a8404c42d | 82 | Acurrent = Acounter_cw - Atemp * 2500; |
ea78anana | 3:4b7a8404c42d | 83 | At = An; |
ea78anana | 3:4b7a8404c42d | 84 | ASet_state(10); |
ea78anana | 3:4b7a8404c42d | 85 | Avelocity = (Atemp * d * pi) / At; |
ea78anana | 3:4b7a8404c42d | 86 | //pc.printf("The A cw_speed is ");pc.printf("%f", Avelocity); pc.printf("m/s. \r\n"); |
ea78anana | 3:4b7a8404c42d | 87 | } |
ea78anana | 3:4b7a8404c42d | 88 | //anti-clockwise turning |
ea78anana | 3:4b7a8404c42d | 89 | else if (Acounter_cw < 0) |
kelhon30 | 0:f5797bc73f93 | 90 | { |
ea78anana | 2:c2106a1bce04 | 91 | Atemp = Acounter_cw / 2500.0; |
kelhon30 | 0:f5797bc73f93 | 92 | At = An; |
ea78anana | 3:4b7a8404c42d | 93 | Acurrent = Acounter_cw - Atemp * 2500; |
ea78anana | 2:c2106a1bce04 | 94 | ASet_state(-10); |
kelhon30 | 1:cb2586b26e9b | 95 | Avelocity = (Atemp * d * pi) / At; |
ea78anana | 3:4b7a8404c42d | 96 | //pc.printf("The A cw_speed is ");pc.printf("%f", Avelocity); pc.printf("m/s. \r\n"); |
kelhon30 | 0:f5797bc73f93 | 97 | } |
kelhon30 | 0:f5797bc73f93 | 98 | } |
ea78anana | 2:c2106a1bce04 | 99 | |
kelhon30 | 1:cb2586b26e9b | 100 | void wheelA_threadA() |
kelhon30 | 1:cb2586b26e9b | 101 | { |
kelhon30 | 1:cb2586b26e9b | 102 | while(1){ |
kelhon30 | 1:cb2586b26e9b | 103 | Aloop(); |
ea78anana | 3:4b7a8404c42d | 104 | Thread::wait(1000); |
kelhon30 | 1:cb2586b26e9b | 105 | //printf("%d %d \r\n", Bcounter_cw, Bcounter_ccw); |
kelhon30 | 1:cb2586b26e9b | 106 | } |
kelhon30 | 1:cb2586b26e9b | 107 | } |
kelhon30 | 0:f5797bc73f93 | 108 | |
kelhon30 | 0:f5797bc73f93 | 109 | //B wheel Variable |
kelhon30 | 0:f5797bc73f93 | 110 | int Bcounter_cw = 0; |
kelhon30 | 0:f5797bc73f93 | 111 | int Bcounter_ccw = 0; |
kelhon30 | 0:f5797bc73f93 | 112 | int Bnum = 0;//number of turns |
kelhon30 | 0:f5797bc73f93 | 113 | double Bt;//time per turn |
kelhon30 | 0:f5797bc73f93 | 114 | float Bvelocity; |
kelhon30 | 0:f5797bc73f93 | 115 | int Bcurrent = 0; |
ea78anana | 3:4b7a8404c42d | 116 | float Btemp = 0; |
kelhon30 | 0:f5797bc73f93 | 117 | int Bn = 0; |
ea78anana | 3:4b7a8404c42d | 118 | float Bdistance = 0; |
kelhon30 | 0:f5797bc73f93 | 119 | double Btime3;//Time of phase Z detected, use for calculate the velocity |
kelhon30 | 0:f5797bc73f93 | 120 | Timer Bf; |
kelhon30 | 0:f5797bc73f93 | 121 | |
kelhon30 | 0:f5797bc73f93 | 122 | DigitalIn b12(Bwheel_B); |
kelhon30 | 0:f5797bc73f93 | 123 | InterruptIn b11(Bwheel_A); |
kelhon30 | 0:f5797bc73f93 | 124 | InterruptIn b13(Bwheel_Z); |
kelhon30 | 0:f5797bc73f93 | 125 | |
kelhon30 | 0:f5797bc73f93 | 126 | void EncodeB() |
kelhon30 | 0:f5797bc73f93 | 127 | { |
ea78anana | 3:4b7a8404c42d | 128 | if((b11 == HIGH) && (b12 == LOW)) |
kelhon30 | 1:cb2586b26e9b | 129 | |
kelhon30 | 0:f5797bc73f93 | 130 | { Bcounter_cw++; |
kelhon30 | 0:f5797bc73f93 | 131 | } |
kelhon30 | 1:cb2586b26e9b | 132 | |
ea78anana | 2:c2106a1bce04 | 133 | else |
kelhon30 | 1:cb2586b26e9b | 134 | { Bcounter_cw--; |
ea78anana | 2:c2106a1bce04 | 135 | |
kelhon30 | 1:cb2586b26e9b | 136 | } |
kelhon30 | 1:cb2586b26e9b | 137 | |
kelhon30 | 0:f5797bc73f93 | 138 | } |
kelhon30 | 0:f5797bc73f93 | 139 | |
kelhon30 | 0:f5797bc73f93 | 140 | void Bsetup(){ |
ea78anana | 3:4b7a8404c42d | 141 | b11.mode(PullUp); |
ea78anana | 3:4b7a8404c42d | 142 | b12.mode(PullUp); |
ea78anana | 3:4b7a8404c42d | 143 | b11.rise(&EncodeB); |
kelhon30 | 0:f5797bc73f93 | 144 | } |
kelhon30 | 1:cb2586b26e9b | 145 | |
ea78anana | 2:c2106a1bce04 | 146 | |
kelhon30 | 1:cb2586b26e9b | 147 | void BSet_state(int a){ |
kelhon30 | 1:cb2586b26e9b | 148 | Bcounter_cw = a; |
kelhon30 | 0:f5797bc73f93 | 149 | Bn = 0; |
kelhon30 | 0:f5797bc73f93 | 150 | } |
kelhon30 | 0:f5797bc73f93 | 151 | |
ea78anana | 2:c2106a1bce04 | 152 | |
kelhon30 | 0:f5797bc73f93 | 153 | void Bloop() |
kelhon30 | 1:cb2586b26e9b | 154 | |
kelhon30 | 0:f5797bc73f93 | 155 | { |
kelhon30 | 0:f5797bc73f93 | 156 | //clockwise turning |
ea78anana | 3:4b7a8404c42d | 157 | Bn = Bn + 1; |
ea78anana | 3:4b7a8404c42d | 158 | if (Bcounter_cw >= 0) |
kelhon30 | 0:f5797bc73f93 | 159 | { |
ea78anana | 2:c2106a1bce04 | 160 | Btemp = Bcounter_cw / 2500.0; |
ea78anana | 3:4b7a8404c42d | 161 | Bcurrent = Bcounter_cw - Btemp * 2500; |
kelhon30 | 0:f5797bc73f93 | 162 | Bt = Bn; |
ea78anana | 2:c2106a1bce04 | 163 | BSet_state(10); |
kelhon30 | 0:f5797bc73f93 | 164 | Bvelocity = (Btemp * d * pi) / Bt; |
ea78anana | 3:4b7a8404c42d | 165 | //pc.printf("The B cw_speed is ");pc.printf("%f", Bvelocity); pc.printf("m/s. \r\n"); |
kelhon30 | 0:f5797bc73f93 | 166 | } |
kelhon30 | 0:f5797bc73f93 | 167 | //anti-clockwise turning |
ea78anana | 3:4b7a8404c42d | 168 | else if (Bcounter_cw < 0) |
kelhon30 | 0:f5797bc73f93 | 169 | { |
ea78anana | 2:c2106a1bce04 | 170 | Btemp = Bcounter_cw / 2500.0; |
ea78anana | 3:4b7a8404c42d | 171 | Bcurrent = Bcounter_cw - Btemp * 2500; |
kelhon30 | 0:f5797bc73f93 | 172 | Bt = Bn; |
ea78anana | 2:c2106a1bce04 | 173 | BSet_state(-10); |
kelhon30 | 1:cb2586b26e9b | 174 | Bvelocity = (Btemp * d * pi) / Bt; |
ea78anana | 3:4b7a8404c42d | 175 | //pc.printf("The B cw_speed is ");pc.printf("%f", Bvelocity); pc.printf("m/s. \r\n"); |
kelhon30 | 0:f5797bc73f93 | 176 | } |
kelhon30 | 0:f5797bc73f93 | 177 | } |
kelhon30 | 1:cb2586b26e9b | 178 | |
kelhon30 | 0:f5797bc73f93 | 179 | void wheelB_threadB() |
kelhon30 | 0:f5797bc73f93 | 180 | { |
kelhon30 | 0:f5797bc73f93 | 181 | while(1){ |
kelhon30 | 0:f5797bc73f93 | 182 | Bloop(); |
ea78anana | 3:4b7a8404c42d | 183 | Thread::wait(1000); |
ea78anana | 3:4b7a8404c42d | 184 | //printf("%d %d \r\n", B2500.0, Bcounter_ccw); |
kelhon30 | 0:f5797bc73f93 | 185 | } |
kelhon30 | 0:f5797bc73f93 | 186 | } |
kelhon30 | 0:f5797bc73f93 | 187 | |
kelhon30 | 0:f5797bc73f93 | 188 | //C wheel Variable |
kelhon30 | 0:f5797bc73f93 | 189 | int Ccounter_cw = 0; |
kelhon30 | 0:f5797bc73f93 | 190 | int Ccounter_ccw = 0; |
kelhon30 | 0:f5797bc73f93 | 191 | int Cnum = 0;//number of turns |
kelhon30 | 0:f5797bc73f93 | 192 | double Ct;//time per turn |
kelhon30 | 0:f5797bc73f93 | 193 | float Cvelocity; |
kelhon30 | 0:f5797bc73f93 | 194 | int Ccurrent = 0; |
ea78anana | 3:4b7a8404c42d | 195 | float Ctemp = 0; |
kelhon30 | 0:f5797bc73f93 | 196 | int Cn = 0; |
ea78anana | 3:4b7a8404c42d | 197 | float Cdistance = 0; |
kelhon30 | 0:f5797bc73f93 | 198 | double Ctime3;//Time of phase Z detected, use for calculate the velocity |
kelhon30 | 0:f5797bc73f93 | 199 | Timer Cf; |
kelhon30 | 0:f5797bc73f93 | 200 | |
kelhon30 | 0:f5797bc73f93 | 201 | DigitalIn c12(Cwheel_B); |
kelhon30 | 0:f5797bc73f93 | 202 | InterruptIn c11(Cwheel_A); |
kelhon30 | 0:f5797bc73f93 | 203 | InterruptIn c13(Cwheel_Z); |
kelhon30 | 0:f5797bc73f93 | 204 | |
kelhon30 | 0:f5797bc73f93 | 205 | void EncodeC() |
kelhon30 | 0:f5797bc73f93 | 206 | { |
ea78anana | 3:4b7a8404c42d | 207 | if((c11 == HIGH) && (c12 == LOW)) |
kelhon30 | 1:cb2586b26e9b | 208 | |
kelhon30 | 0:f5797bc73f93 | 209 | { Ccounter_cw++; |
kelhon30 | 0:f5797bc73f93 | 210 | } |
kelhon30 | 1:cb2586b26e9b | 211 | |
ea78anana | 2:c2106a1bce04 | 212 | else |
kelhon30 | 1:cb2586b26e9b | 213 | { Ccounter_cw--; |
ea78anana | 2:c2106a1bce04 | 214 | |
kelhon30 | 1:cb2586b26e9b | 215 | } |
kelhon30 | 1:cb2586b26e9b | 216 | |
kelhon30 | 0:f5797bc73f93 | 217 | } |
kelhon30 | 0:f5797bc73f93 | 218 | |
kelhon30 | 0:f5797bc73f93 | 219 | void Csetup(){ |
ea78anana | 3:4b7a8404c42d | 220 | c11.mode(PullUp); |
ea78anana | 3:4b7a8404c42d | 221 | c12.mode(PullUp); |
ea78anana | 3:4b7a8404c42d | 222 | c11.rise(&EncodeC); |
kelhon30 | 0:f5797bc73f93 | 223 | } |
kelhon30 | 1:cb2586b26e9b | 224 | |
ea78anana | 2:c2106a1bce04 | 225 | |
kelhon30 | 1:cb2586b26e9b | 226 | void CSet_state(int a){ |
kelhon30 | 1:cb2586b26e9b | 227 | Ccounter_cw = a; |
kelhon30 | 0:f5797bc73f93 | 228 | Cn = 0; |
kelhon30 | 0:f5797bc73f93 | 229 | } |
kelhon30 | 0:f5797bc73f93 | 230 | |
ea78anana | 2:c2106a1bce04 | 231 | |
kelhon30 | 0:f5797bc73f93 | 232 | void Cloop() |
kelhon30 | 1:cb2586b26e9b | 233 | |
kelhon30 | 0:f5797bc73f93 | 234 | { |
kelhon30 | 0:f5797bc73f93 | 235 | //clockwise turning |
ea78anana | 3:4b7a8404c42d | 236 | Cn = Cn + 1; |
ea78anana | 3:4b7a8404c42d | 237 | if (Ccounter_cw >= 0) |
kelhon30 | 0:f5797bc73f93 | 238 | { |
ea78anana | 2:c2106a1bce04 | 239 | Ctemp = Ccounter_cw / 2500.0; |
ea78anana | 3:4b7a8404c42d | 240 | Ccurrent = Ccounter_cw - Ctemp * 2500; |
kelhon30 | 0:f5797bc73f93 | 241 | Ct = Cn; |
ea78anana | 2:c2106a1bce04 | 242 | CSet_state(10); |
kelhon30 | 0:f5797bc73f93 | 243 | Cvelocity = (Ctemp * d * pi) / Ct; |
ea78anana | 3:4b7a8404c42d | 244 | //pc.printf("The C cw_speed is ");pc.printf("%f", Cvelocity); pc.printf("m/s. \r\n"); |
kelhon30 | 0:f5797bc73f93 | 245 | } |
kelhon30 | 0:f5797bc73f93 | 246 | //anti-clockwise turning |
ea78anana | 3:4b7a8404c42d | 247 | else if (Ccounter_cw < 0) |
kelhon30 | 0:f5797bc73f93 | 248 | { |
ea78anana | 2:c2106a1bce04 | 249 | Ctemp = Ccounter_cw / 2500.0; |
ea78anana | 3:4b7a8404c42d | 250 | Ccurrent = Ccounter_cw - Ctemp * 2500; |
kelhon30 | 0:f5797bc73f93 | 251 | Ct = Cn; |
ea78anana | 2:c2106a1bce04 | 252 | CSet_state(-10); |
kelhon30 | 1:cb2586b26e9b | 253 | Cvelocity = (Ctemp * d * pi) / Ct; |
ea78anana | 3:4b7a8404c42d | 254 | //pc.printf("The C cw_speed is ");pc.printf("%f", Cvelocity); pc.printf("m/s. \r\n"); |
kelhon30 | 0:f5797bc73f93 | 255 | } |
kelhon30 | 0:f5797bc73f93 | 256 | } |
kelhon30 | 1:cb2586b26e9b | 257 | |
kelhon30 | 0:f5797bc73f93 | 258 | void wheelC_threadC() |
kelhon30 | 0:f5797bc73f93 | 259 | { |
kelhon30 | 0:f5797bc73f93 | 260 | while(1){ |
kelhon30 | 0:f5797bc73f93 | 261 | Cloop(); |
ea78anana | 3:4b7a8404c42d | 262 | Thread::wait(1000); |
kelhon30 | 1:cb2586b26e9b | 263 | //printf("%d %d \r\n", Ccounter_cw, Ccounter_ccw); |
kelhon30 | 0:f5797bc73f93 | 264 | } |
kelhon30 | 0:f5797bc73f93 | 265 | } |
kelhon30 | 0:f5797bc73f93 | 266 | |
kelhon30 | 0:f5797bc73f93 | 267 | //calculation part |
kelhon30 | 0:f5797bc73f93 | 268 | |
kelhon30 | 0:f5797bc73f93 | 269 | |
kelhon30 | 0:f5797bc73f93 | 270 | void calvector() |
kelhon30 | 0:f5797bc73f93 | 271 | { |
kelhon30 | 0:f5797bc73f93 | 272 | float v[3] = {Avelocity, Bvelocity, Cvelocity}; |
kelhon30 | 1:cb2586b26e9b | 273 | float x; |
ea78anana | 3:4b7a8404c42d | 274 | float y; |
kelhon30 | 1:cb2586b26e9b | 275 | x = sqrt(3.0); |
ea78anana | 3:4b7a8404c42d | 276 | float r = 11; |
kelhon30 | 0:f5797bc73f93 | 277 | float b[3]; |
kelhon30 | 0:f5797bc73f93 | 278 | float a[3][3] = |
kelhon30 | 0:f5797bc73f93 | 279 | { |
kelhon30 | 0:f5797bc73f93 | 280 | {(-(2/3.0)), (1/3.0), (1/3.0)}, |
kelhon30 | 1:cb2586b26e9b | 281 | {0, (-(x)/3), ((x)/3)}, |
kelhon30 | 0:f5797bc73f93 | 282 | {(1/(3*r)), (1/(3*r)), (1/(3*r))}}; |
kelhon30 | 0:f5797bc73f93 | 283 | //for ( int i = 0; i < 3; i++ ) |
kelhon30 | 0:f5797bc73f93 | 284 | //for ( int j = 0; j < 3; j++ ) { |
kelhon30 | 1:cb2586b26e9b | 285 | |
kelhon30 | 0:f5797bc73f93 | 286 | //cout << "a[" << i << "][" << j << "]: "; |
kelhon30 | 0:f5797bc73f93 | 287 | //cout << a[i][j]<< endl;} |
kelhon30 | 0:f5797bc73f93 | 288 | //multiple matrix |
kelhon30 | 0:f5797bc73f93 | 289 | for (int i=0; i<3; i++){ |
kelhon30 | 1:cb2586b26e9b | 290 | b[i] = ((a[i][0]*v[0])+(a[i][1]*v[1])+(a[i][2]*v[2])); |
ea78anana | 3:4b7a8404c42d | 291 | printf(" %f \r\n", b[i]); |
kelhon30 | 0:f5797bc73f93 | 292 | } |
ea78anana | 3:4b7a8404c42d | 293 | printf("\r\n"); |
ea78anana | 3:4b7a8404c42d | 294 | y = sqrt( (b[0] * b[0]) + (b[1] * b[1])); |
ea78anana | 3:4b7a8404c42d | 295 | printf("Magnitude: %f \r\n", y); |
ea78anana | 3:4b7a8404c42d | 296 | for (int i=0; i<3; i++){ |
ea78anana | 3:4b7a8404c42d | 297 | b[i] = 0; |
ea78anana | 3:4b7a8404c42d | 298 | } |
ea78anana | 3:4b7a8404c42d | 299 | printf("\r\n"); |
kelhon30 | 0:f5797bc73f93 | 300 | } |
kelhon30 | 0:f5797bc73f93 | 301 | |
kelhon30 | 0:f5797bc73f93 | 302 | int main() |
kelhon30 | 0:f5797bc73f93 | 303 | { |
kelhon30 | 0:f5797bc73f93 | 304 | pc.printf("start"); |
kelhon30 | 0:f5797bc73f93 | 305 | Asetup(); |
kelhon30 | 0:f5797bc73f93 | 306 | Af.start(); |
kelhon30 | 1:cb2586b26e9b | 307 | threadA.start(wheelA_threadA); |
kelhon30 | 0:f5797bc73f93 | 308 | Bsetup(); |
kelhon30 | 0:f5797bc73f93 | 309 | Bf.start(); |
kelhon30 | 0:f5797bc73f93 | 310 | threadB.start(wheelB_threadB); |
kelhon30 | 0:f5797bc73f93 | 311 | Csetup(); |
kelhon30 | 0:f5797bc73f93 | 312 | Cf.start(); |
kelhon30 | 0:f5797bc73f93 | 313 | threadC.start(wheelC_threadC); |
kelhon30 | 0:f5797bc73f93 | 314 | while(1) |
kelhon30 | 0:f5797bc73f93 | 315 | { |
ea78anana | 3:4b7a8404c42d | 316 | Thread::wait(1000); |
kelhon30 | 0:f5797bc73f93 | 317 | calvector(); |
kelhon30 | 1:cb2586b26e9b | 318 | //pc.printf("%d %d \r\n", Acounter_cw, Acounter_ccw); |
kelhon30 | 0:f5797bc73f93 | 319 | } |
kelhon30 | 0:f5797bc73f93 | 320 | } |