Sik Chiu Chow / Mbed 2 deprecated test2

Triangular Omni-wheels

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
Child:
2:c2106a1bce04
asdasd

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"
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 }