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