Sik Chiu Chow / Mbed 2 deprecated test2

Triangular Omni-wheels

Dependencies:   mbed Test2Boards

main.cpp@4:013f9a62dec8, 2021-11-28 (annotated)

Committer:
ea78anana
Date:
Sun Nov 28 14:13:33 2021 +0000
Revision:
4:013f9a62dec8
Parent:
3:4b7a8404c42d 
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 Anum = 0;//number of turns
kelhon30 0:f5797bc73f93 33 double At;//time per turn
kelhon30 0:f5797bc73f93 34 float Avelocity;
kelhon30 0:f5797bc73f93 35 int Acurrent = 0;
ea78anana 3:4b7a8404c42d 36 float Atemp = 0;
kelhon30 0:f5797bc73f93 37 int An = 0;
ea78anana 3:4b7a8404c42d 38 float Adistance = 0;
kelhon30 0:f5797bc73f93 39
kelhon30 0:f5797bc73f93 40 DigitalIn a12(Awheel_B);
kelhon30 0:f5797bc73f93 41 InterruptIn a11(Awheel_A);
kelhon30 0:f5797bc73f93 42 InterruptIn a13(Awheel_Z);
kelhon30 0:f5797bc73f93 43
kelhon30 0:f5797bc73f93 44 void EncodeA()
kelhon30 0:f5797bc73f93 45 {
kelhon30 0:f5797bc73f93 46 if((a11 == HIGH) && (a12 == LOW))
kelhon30 1:cb2586b26e9b 47
kelhon30 0:f5797bc73f93 48 { Acounter_cw++;
kelhon30 0:f5797bc73f93 49 }
kelhon30 1:cb2586b26e9b 50
ea78anana 2:c2106a1bce04 51 else
kelhon30 1:cb2586b26e9b 52 { Acounter_cw--;
ea78anana 2:c2106a1bce04 53
kelhon30 1:cb2586b26e9b 54 }
kelhon30 1:cb2586b26e9b 55
kelhon30 0:f5797bc73f93 56 }
kelhon30 0:f5797bc73f93 57
kelhon30 0:f5797bc73f93 58 void Asetup(){
kelhon30 0:f5797bc73f93 59 a11.mode(PullUp);
kelhon30 0:f5797bc73f93 60 a12.mode(PullUp);
kelhon30 0:f5797bc73f93 61 a11.rise(&EncodeA);
kelhon30 0:f5797bc73f93 62 }
kelhon30 1:cb2586b26e9b 63
ea78anana 2:c2106a1bce04 64
kelhon30 1:cb2586b26e9b 65 void ASet_state(int a){
kelhon30 1:cb2586b26e9b 66 Acounter_cw = a;
kelhon30 0:f5797bc73f93 67 An = 0;
kelhon30 0:f5797bc73f93 68 }
kelhon30 0:f5797bc73f93 69
ea78anana 2:c2106a1bce04 70
kelhon30 0:f5797bc73f93 71 void Aloop()
kelhon30 1:cb2586b26e9b 72
kelhon30 0:f5797bc73f93 73 {
kelhon30 0:f5797bc73f93 74 //clockwise turning
ea78anana 4:013f9a62dec8 75 An = An + 1;//count the pulses per second
ea78anana 3:4b7a8404c42d 76 if (Acounter_cw >= 0)
ea78anana 3:4b7a8404c42d 77 {
ea78anana 3:4b7a8404c42d 78 Atemp = Acounter_cw / 2500.0;
ea78anana 3:4b7a8404c42d 79 Acurrent = Acounter_cw - Atemp * 2500;
ea78anana 3:4b7a8404c42d 80 At = An;
ea78anana 4:013f9a62dec8 81 ASet_state(10);//keeping clockwise direction
ea78anana 3:4b7a8404c42d 82 Avelocity = (Atemp * d * pi) / At;
ea78anana 3:4b7a8404c42d 83 }
ea78anana 3:4b7a8404c42d 84 //anti-clockwise turning
ea78anana 3:4b7a8404c42d 85 else if (Acounter_cw < 0)
kelhon30 0:f5797bc73f93 86 {
ea78anana 2:c2106a1bce04 87 Atemp = Acounter_cw / 2500.0;
kelhon30 0:f5797bc73f93 88 At = An;
ea78anana 3:4b7a8404c42d 89 Acurrent = Acounter_cw - Atemp * 2500;
ea78anana 4:013f9a62dec8 90 ASet_state(-10);//keeping anti-clockwise direction
kelhon30 1:cb2586b26e9b 91 Avelocity = (Atemp * d * pi) / At;
kelhon30 0:f5797bc73f93 92 }
kelhon30 0:f5797bc73f93 93 }
ea78anana 2:c2106a1bce04 94
kelhon30 1:cb2586b26e9b 95 void wheelA_threadA()
kelhon30 1:cb2586b26e9b 96 {
kelhon30 1:cb2586b26e9b 97 while(1){
kelhon30 1:cb2586b26e9b 98 Aloop();
ea78anana 3:4b7a8404c42d 99 Thread::wait(1000);
kelhon30 1:cb2586b26e9b 100 }
kelhon30 1:cb2586b26e9b 101 }
kelhon30 0:f5797bc73f93 102
kelhon30 0:f5797bc73f93 103 //B wheel Variable
kelhon30 0:f5797bc73f93 104 int Bcounter_cw = 0;
kelhon30 0:f5797bc73f93 105 int Bnum = 0;//number of turns
kelhon30 0:f5797bc73f93 106 double Bt;//time per turn
kelhon30 0:f5797bc73f93 107 float Bvelocity;
kelhon30 0:f5797bc73f93 108 int Bcurrent = 0;
ea78anana 3:4b7a8404c42d 109 float Btemp = 0;
kelhon30 0:f5797bc73f93 110 int Bn = 0;
kelhon30 0:f5797bc73f93 111 Timer Bf;
kelhon30 0:f5797bc73f93 112
kelhon30 0:f5797bc73f93 113 DigitalIn b12(Bwheel_B);
kelhon30 0:f5797bc73f93 114 InterruptIn b11(Bwheel_A);
kelhon30 0:f5797bc73f93 115 InterruptIn b13(Bwheel_Z);
kelhon30 0:f5797bc73f93 116
kelhon30 0:f5797bc73f93 117 void EncodeB()
kelhon30 0:f5797bc73f93 118 {
ea78anana 3:4b7a8404c42d 119 if((b11 == HIGH) && (b12 == LOW))
kelhon30 1:cb2586b26e9b 120
kelhon30 0:f5797bc73f93 121 { Bcounter_cw++;
kelhon30 0:f5797bc73f93 122 }
kelhon30 1:cb2586b26e9b 123
ea78anana 2:c2106a1bce04 124 else
kelhon30 1:cb2586b26e9b 125 { Bcounter_cw--;
ea78anana 2:c2106a1bce04 126
kelhon30 1:cb2586b26e9b 127 }
kelhon30 1:cb2586b26e9b 128
kelhon30 0:f5797bc73f93 129 }
kelhon30 0:f5797bc73f93 130
kelhon30 0:f5797bc73f93 131 void Bsetup(){
ea78anana 3:4b7a8404c42d 132 b11.mode(PullUp);
ea78anana 3:4b7a8404c42d 133 b12.mode(PullUp);
ea78anana 3:4b7a8404c42d 134 b11.rise(&EncodeB);
kelhon30 0:f5797bc73f93 135 }
kelhon30 1:cb2586b26e9b 136
ea78anana 2:c2106a1bce04 137
kelhon30 1:cb2586b26e9b 138 void BSet_state(int a){
kelhon30 1:cb2586b26e9b 139 Bcounter_cw = a;
kelhon30 0:f5797bc73f93 140 Bn = 0;
kelhon30 0:f5797bc73f93 141 }
kelhon30 0:f5797bc73f93 142
ea78anana 2:c2106a1bce04 143
kelhon30 0:f5797bc73f93 144 void Bloop()
kelhon30 1:cb2586b26e9b 145
kelhon30 0:f5797bc73f93 146 {
kelhon30 0:f5797bc73f93 147 //clockwise turning
ea78anana 3:4b7a8404c42d 148 Bn = Bn + 1;
ea78anana 3:4b7a8404c42d 149 if (Bcounter_cw >= 0)
kelhon30 0:f5797bc73f93 150 {
ea78anana 2:c2106a1bce04 151 Btemp = Bcounter_cw / 2500.0;
ea78anana 3:4b7a8404c42d 152 Bcurrent = Bcounter_cw - Btemp * 2500;
kelhon30 0:f5797bc73f93 153 Bt = Bn;
ea78anana 4:013f9a62dec8 154 BSet_state(10);//keeping clockwise direction
kelhon30 0:f5797bc73f93 155 Bvelocity = (Btemp * d * pi) / Bt;
kelhon30 0:f5797bc73f93 156 }
kelhon30 0:f5797bc73f93 157 //anti-clockwise turning
ea78anana 3:4b7a8404c42d 158 else 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 4:013f9a62dec8 163 BSet_state(-10);//keeping anti-clockwise direction
kelhon30 1:cb2586b26e9b 164 Bvelocity = (Btemp * d * pi) / Bt;
kelhon30 0:f5797bc73f93 165 }
kelhon30 0:f5797bc73f93 166 }
kelhon30 1:cb2586b26e9b 167
kelhon30 0:f5797bc73f93 168 void wheelB_threadB()
kelhon30 0:f5797bc73f93 169 {
kelhon30 0:f5797bc73f93 170 while(1){
kelhon30 0:f5797bc73f93 171 Bloop();
ea78anana 3:4b7a8404c42d 172 Thread::wait(1000);
kelhon30 0:f5797bc73f93 173 }
kelhon30 0:f5797bc73f93 174 }
kelhon30 0:f5797bc73f93 175
kelhon30 0:f5797bc73f93 176 //C wheel Variable
kelhon30 0:f5797bc73f93 177 int Ccounter_cw = 0;
kelhon30 0:f5797bc73f93 178 int Cnum = 0;//number of turns
kelhon30 0:f5797bc73f93 179 double Ct;//time per turn
kelhon30 0:f5797bc73f93 180 float Cvelocity;
kelhon30 0:f5797bc73f93 181 int Ccurrent = 0;
ea78anana 3:4b7a8404c42d 182 float Ctemp = 0;
kelhon30 0:f5797bc73f93 183 int Cn = 0;
kelhon30 0:f5797bc73f93 184 Timer Cf;
kelhon30 0:f5797bc73f93 185
kelhon30 0:f5797bc73f93 186 DigitalIn c12(Cwheel_B);
kelhon30 0:f5797bc73f93 187 InterruptIn c11(Cwheel_A);
kelhon30 0:f5797bc73f93 188 InterruptIn c13(Cwheel_Z);
kelhon30 0:f5797bc73f93 189
kelhon30 0:f5797bc73f93 190 void EncodeC()
kelhon30 0:f5797bc73f93 191 {
ea78anana 3:4b7a8404c42d 192 if((c11 == HIGH) && (c12 == LOW))
kelhon30 1:cb2586b26e9b 193
kelhon30 0:f5797bc73f93 194 { Ccounter_cw++;
kelhon30 0:f5797bc73f93 195 }
kelhon30 1:cb2586b26e9b 196
ea78anana 2:c2106a1bce04 197 else
kelhon30 1:cb2586b26e9b 198 { Ccounter_cw--;
ea78anana 2:c2106a1bce04 199
kelhon30 1:cb2586b26e9b 200 }
kelhon30 1:cb2586b26e9b 201
kelhon30 0:f5797bc73f93 202 }
kelhon30 0:f5797bc73f93 203
kelhon30 0:f5797bc73f93 204 void Csetup(){
ea78anana 3:4b7a8404c42d 205 c11.mode(PullUp);
ea78anana 3:4b7a8404c42d 206 c12.mode(PullUp);
ea78anana 3:4b7a8404c42d 207 c11.rise(&EncodeC);
kelhon30 0:f5797bc73f93 208 }
kelhon30 1:cb2586b26e9b 209
ea78anana 2:c2106a1bce04 210
kelhon30 1:cb2586b26e9b 211 void CSet_state(int a){
kelhon30 1:cb2586b26e9b 212 Ccounter_cw = a;
kelhon30 0:f5797bc73f93 213 Cn = 0;
kelhon30 0:f5797bc73f93 214 }
kelhon30 0:f5797bc73f93 215
ea78anana 2:c2106a1bce04 216
kelhon30 0:f5797bc73f93 217 void Cloop()
kelhon30 1:cb2586b26e9b 218
kelhon30 0:f5797bc73f93 219 {
kelhon30 0:f5797bc73f93 220 //clockwise turning
ea78anana 3:4b7a8404c42d 221 Cn = Cn + 1;
ea78anana 3:4b7a8404c42d 222 if (Ccounter_cw >= 0)
kelhon30 0:f5797bc73f93 223 {
ea78anana 2:c2106a1bce04 224 Ctemp = Ccounter_cw / 2500.0;
ea78anana 3:4b7a8404c42d 225 Ccurrent = Ccounter_cw - Ctemp * 2500;
kelhon30 0:f5797bc73f93 226 Ct = Cn;
ea78anana 4:013f9a62dec8 227 CSet_state(10);//keeping clockwise direction
kelhon30 0:f5797bc73f93 228 Cvelocity = (Ctemp * d * pi) / Ct;
kelhon30 0:f5797bc73f93 229 }
kelhon30 0:f5797bc73f93 230 //anti-clockwise turning
ea78anana 3:4b7a8404c42d 231 else if (Ccounter_cw < 0)
kelhon30 0:f5797bc73f93 232 {
ea78anana 2:c2106a1bce04 233 Ctemp = Ccounter_cw / 2500.0;
ea78anana 3:4b7a8404c42d 234 Ccurrent = Ccounter_cw - Ctemp * 2500;
kelhon30 0:f5797bc73f93 235 Ct = Cn;
ea78anana 4:013f9a62dec8 236 CSet_state(-10);//keeping anti-clockwise direction
kelhon30 1:cb2586b26e9b 237 Cvelocity = (Ctemp * d * pi) / Ct;
kelhon30 0:f5797bc73f93 238 }
kelhon30 0:f5797bc73f93 239 }
kelhon30 1:cb2586b26e9b 240
kelhon30 0:f5797bc73f93 241 void wheelC_threadC()
kelhon30 0:f5797bc73f93 242 {
kelhon30 0:f5797bc73f93 243 while(1){
kelhon30 0:f5797bc73f93 244 Cloop();
ea78anana 3:4b7a8404c42d 245 Thread::wait(1000);
kelhon30 0:f5797bc73f93 246 }
kelhon30 0:f5797bc73f93 247 }
kelhon30 0:f5797bc73f93 248
kelhon30 0:f5797bc73f93 249 //calculation part
kelhon30 0:f5797bc73f93 250
kelhon30 0:f5797bc73f93 251
kelhon30 0:f5797bc73f93 252 void calvector()
kelhon30 0:f5797bc73f93 253 {
kelhon30 0:f5797bc73f93 254 float v[3] = {Avelocity, Bvelocity, Cvelocity};
kelhon30 1:cb2586b26e9b 255 float x;
ea78anana 3:4b7a8404c42d 256 float y;
kelhon30 1:cb2586b26e9b 257 x = sqrt(3.0);
ea78anana 3:4b7a8404c42d 258 float r = 11;
kelhon30 0:f5797bc73f93 259 float b[3];
kelhon30 0:f5797bc73f93 260 float a[3][3] =
kelhon30 0:f5797bc73f93 261 {
kelhon30 0:f5797bc73f93 262 {(-(2/3.0)), (1/3.0), (1/3.0)},
kelhon30 1:cb2586b26e9b 263 {0, (-(x)/3), ((x)/3)},
kelhon30 0:f5797bc73f93 264 {(1/(3*r)), (1/(3*r)), (1/(3*r))}};
ea78anana 4:013f9a62dec8 265
ea78anana 4:013f9a62dec8 266 //multiple matrix calculating(Vx, Vy, Angular velocity)
kelhon30 0:f5797bc73f93 267 for (int i=0; i<3; i++){
kelhon30 1:cb2586b26e9b 268 b[i] = ((a[i][0]*v[0])+(a[i][1]*v[1])+(a[i][2]*v[2]));
ea78anana 3:4b7a8404c42d 269 printf(" %f \r\n", b[i]);
ea78anana 4:013f9a62dec8 270 }
ea78anana 3:4b7a8404c42d 271 printf("\r\n");
ea78anana 4:013f9a62dec8 272
ea78anana 4:013f9a62dec8 273 y = sqrt( (b[0] * b[0]) + (b[1] * b[1]));//Magnitude of the vector
ea78anana 3:4b7a8404c42d 274 printf("Magnitude: %f \r\n", y);
ea78anana 3:4b7a8404c42d 275 for (int i=0; i<3; i++){
ea78anana 3:4b7a8404c42d 276 b[i] = 0;
ea78anana 3:4b7a8404c42d 277 }
ea78anana 3:4b7a8404c42d 278 printf("\r\n");
kelhon30 0:f5797bc73f93 279 }
kelhon30 0:f5797bc73f93 280
kelhon30 0:f5797bc73f93 281 int main()
kelhon30 0:f5797bc73f93 282 {
kelhon30 0:f5797bc73f93 283 pc.printf("start");
kelhon30 0:f5797bc73f93 284 Asetup();
kelhon30 0:f5797bc73f93 285 Af.start();
kelhon30 1:cb2586b26e9b 286 threadA.start(wheelA_threadA);
kelhon30 0:f5797bc73f93 287 Bsetup();
kelhon30 0:f5797bc73f93 288 Bf.start();
kelhon30 0:f5797bc73f93 289 threadB.start(wheelB_threadB);
kelhon30 0:f5797bc73f93 290 Csetup();
kelhon30 0:f5797bc73f93 291 Cf.start();
kelhon30 0:f5797bc73f93 292 threadC.start(wheelC_threadC);
kelhon30 0:f5797bc73f93 293 while(1)
kelhon30 0:f5797bc73f93 294 {
ea78anana 3:4b7a8404c42d 295 Thread::wait(1000);
kelhon30 0:f5797bc73f93 296 calvector();
kelhon30 0:f5797bc73f93 297 }
kelhon30 0:f5797bc73f93 298 }