Sik Chiu Chow / Mbed 2 deprecated test2

Triangular Omni-wheels

Dependencies:   mbed Test2Boards

main.cpp@2:c2106a1bce04, 2021-11-26 (annotated)

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