Sik Chiu Chow / Mbed 2 deprecated test2

Triangular Omni-wheels

Dependencies:   mbed Test2Boards

main.cpp@0:f5797bc73f93, 2021-11-26 (annotated)

Committer:
kelhon30
Date:
Fri Nov 26 14:22:16 2021 +0000
Revision:
0:f5797bc73f93
Child:
1:cb2586b26e9b
abc

Who changed what in which revision?

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