Sik Chiu Chow / Mbed 2 deprecated test2

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?

UserRevisionLine numberNew 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 }