![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
sdf
Dependencies: mbed arrc_mbed BNO055 SDFileSystem NK_mbed
main.cpp@0:e32543d6e1e4, 2021-08-22 (annotated)
- Committer:
- hamohamo
- Date:
- Sun Aug 22 13:02:21 2021 +0000
- Revision:
- 0:e32543d6e1e4
p
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
hamohamo | 0:e32543d6e1e4 | 1 | /*INCLUDE*/ |
hamohamo | 0:e32543d6e1e4 | 2 | #include "SDFileSystem.h" |
hamohamo | 0:e32543d6e1e4 | 3 | #include "mbed.h" |
hamohamo | 0:e32543d6e1e4 | 4 | #include "rotary_inc.hpp" |
hamohamo | 0:e32543d6e1e4 | 5 | #include "matrix.h" |
hamohamo | 0:e32543d6e1e4 | 6 | #include "BNO055.h" |
hamohamo | 0:e32543d6e1e4 | 7 | #include "NK_PID.hpp" |
hamohamo | 0:e32543d6e1e4 | 8 | #include <cmath> |
hamohamo | 0:e32543d6e1e4 | 9 | #include <vector> |
hamohamo | 0:e32543d6e1e4 | 10 | #include "scrp_slave.hpp" |
hamohamo | 0:e32543d6e1e4 | 11 | |
hamohamo | 0:e32543d6e1e4 | 12 | ScrpSlave sendpwm(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800); |
hamohamo | 0:e32543d6e1e4 | 13 | |
hamohamo | 0:e32543d6e1e4 | 14 | struct pos{ |
hamohamo | 0:e32543d6e1e4 | 15 | double x; |
hamohamo | 0:e32543d6e1e4 | 16 | double y; |
hamohamo | 0:e32543d6e1e4 | 17 | }; |
hamohamo | 0:e32543d6e1e4 | 18 | /*FUNCTIONS*/ |
hamohamo | 0:e32543d6e1e4 | 19 | void route_read(); |
hamohamo | 0:e32543d6e1e4 | 20 | void odmetry(); |
hamohamo | 0:e32543d6e1e4 | 21 | void move(double vx,double vy,double vw); |
hamohamo | 0:e32543d6e1e4 | 22 | void getpoint(pos *p,int n); |
hamohamo | 0:e32543d6e1e4 | 23 | void getvelocity(double *v,int n); |
hamohamo | 0:e32543d6e1e4 | 24 | |
hamohamo | 0:e32543d6e1e4 | 25 | /*DECLARATION*/ |
hamohamo | 0:e32543d6e1e4 | 26 | SDFileSystem sd(PB_15, PB_14, PB_13, PB_2,"sd"); |
hamohamo | 0:e32543d6e1e4 | 27 | |
hamohamo | 0:e32543d6e1e4 | 28 | RotaryInc cenc0(PC_10,PC_11,2*M_PI,256,4); |
hamohamo | 0:e32543d6e1e4 | 29 | RotaryInc cenc1(PC_4,PA_13,2*M_PI,256,4); |
hamohamo | 0:e32543d6e1e4 | 30 | RotaryInc cenc2(PA_14,PA_15,2*M_PI,256,4); |
hamohamo | 0:e32543d6e1e4 | 31 | RotaryInc cenc3(PC_2,PC_3,2*M_PI,256,4); |
hamohamo | 0:e32543d6e1e4 | 32 | |
hamohamo | 0:e32543d6e1e4 | 33 | RotaryInc senc0(PA_6,PA_7,2*M_PI,256,4); |
hamohamo | 0:e32543d6e1e4 | 34 | RotaryInc senc1(PA_8,PA_9,2*M_PI,256,4); |
hamohamo | 0:e32543d6e1e4 | 35 | RotaryInc senc2(PC_0,PC_1,2*M_PI,256,4); |
hamohamo | 0:e32543d6e1e4 | 36 | RotaryInc senc3(PA_12,PC_5,2*M_PI,256,4); |
hamohamo | 0:e32543d6e1e4 | 37 | |
hamohamo | 0:e32543d6e1e4 | 38 | std::vector<double> t_info,x_info,y_info,v_info,theta_info; |
hamohamo | 0:e32543d6e1e4 | 39 | |
hamohamo | 0:e32543d6e1e4 | 40 | /* |
hamohamo | 0:e32543d6e1e4 | 41 | 01 |
hamohamo | 0:e32543d6e1e4 | 42 | 32 |
hamohamo | 0:e32543d6e1e4 | 43 | */ |
hamohamo | 0:e32543d6e1e4 | 44 | |
hamohamo | 0:e32543d6e1e4 | 45 | Nk_pid mot0(0.0067,0.001,0,0.01); |
hamohamo | 0:e32543d6e1e4 | 46 | Nk_pid mot1(0.0067,0.001,0,0.01); |
hamohamo | 0:e32543d6e1e4 | 47 | Nk_pid mot2(0.0067,0.001,0,0.01); |
hamohamo | 0:e32543d6e1e4 | 48 | Nk_pid mot3(0.0067,0.001,0,0.01); |
hamohamo | 0:e32543d6e1e4 | 49 | |
hamohamo | 0:e32543d6e1e4 | 50 | BNO055 bno(PB_3, PB_10); |
hamohamo | 0:e32543d6e1e4 | 51 | |
hamohamo | 0:e32543d6e1e4 | 52 | Timer LOOPdt; |
hamohamo | 0:e32543d6e1e4 | 53 | unsigned long long int t; |
hamohamo | 0:e32543d6e1e4 | 54 | |
hamohamo | 0:e32543d6e1e4 | 55 | pos a,b,c; |
hamohamo | 0:e32543d6e1e4 | 56 | double v = 0; |
hamohamo | 0:e32543d6e1e4 | 57 | double cwv[4]; |
hamohamo | 0:e32543d6e1e4 | 58 | double swv[4]; |
hamohamo | 0:e32543d6e1e4 | 59 | double theta = 0; |
hamohamo | 0:e32543d6e1e4 | 60 | double r = 50.6; |
hamohamo | 0:e32543d6e1e4 | 61 | double R = 2.0*r; |
hamohamo | 0:e32543d6e1e4 | 62 | double cR = 322.5; |
hamohamo | 0:e32543d6e1e4 | 63 | int n = 0; |
hamohamo | 0:e32543d6e1e4 | 64 | |
hamohamo | 0:e32543d6e1e4 | 65 | PwmOut asd(PC_6); |
hamohamo | 0:e32543d6e1e4 | 66 | |
hamohamo | 0:e32543d6e1e4 | 67 | int main(){ |
hamohamo | 0:e32543d6e1e4 | 68 | asd = 1; |
hamohamo | 0:e32543d6e1e4 | 69 | /*SETUP*/ |
hamohamo | 0:e32543d6e1e4 | 70 | LOOPdt.start(); |
hamohamo | 0:e32543d6e1e4 | 71 | /* |
hamohamo | 0:e32543d6e1e4 | 72 | */ |
hamohamo | 0:e32543d6e1e4 | 73 | mot0.SetLimit(0.5,0.00,-0.5,-0.00); |
hamohamo | 0:e32543d6e1e4 | 74 | mot1.SetLimit(0.5,0.00,-0.5,-0.00); |
hamohamo | 0:e32543d6e1e4 | 75 | mot2.SetLimit(0.5,0.00,-0.5,-0.00); |
hamohamo | 0:e32543d6e1e4 | 76 | mot3.SetLimit(0.5,0.00,-0.5,-0.00); |
hamohamo | 0:e32543d6e1e4 | 77 | /*0.07*/ |
hamohamo | 0:e32543d6e1e4 | 78 | |
hamohamo | 0:e32543d6e1e4 | 79 | //route_read(); |
hamohamo | 0:e32543d6e1e4 | 80 | odmetry(); |
hamohamo | 0:e32543d6e1e4 | 81 | /*LOOP*/ |
hamohamo | 0:e32543d6e1e4 | 82 | wait(3); |
hamohamo | 0:e32543d6e1e4 | 83 | while(true){ |
hamohamo | 0:e32543d6e1e4 | 84 | |
hamohamo | 0:e32543d6e1e4 | 85 | odmetry(); |
hamohamo | 0:e32543d6e1e4 | 86 | /* |
hamohamo | 0:e32543d6e1e4 | 87 | if(sqrt( (a.x-c.x)*(a.x-c.x)+(a.y-c.y)*(a.y-c.y) ) < 30){ |
hamohamo | 0:e32543d6e1e4 | 88 | getpoint(&a,n); |
hamohamo | 0:e32543d6e1e4 | 89 | getvelocity(&v,n); |
hamohamo | 0:e32543d6e1e4 | 90 | if(n<x_info.size()) n++; |
hamohamo | 0:e32543d6e1e4 | 91 | printf("pass\n"); |
hamohamo | 0:e32543d6e1e4 | 92 | } |
hamohamo | 0:e32543d6e1e4 | 93 | double theta = atan2(a.y-c.y,a.x-c.x); |
hamohamo | 0:e32543d6e1e4 | 94 | move(v*cos(theta),v*sin(theta),0);*/ |
hamohamo | 0:e32543d6e1e4 | 95 | //move(0.0,2000.0,0.0); |
hamohamo | 0:e32543d6e1e4 | 96 | //motor0.pwmout(0.2); |
hamohamo | 0:e32543d6e1e4 | 97 | //printf("%lf\n",cwv[0]*r); |
hamohamo | 0:e32543d6e1e4 | 98 | |
hamohamo | 0:e32543d6e1e4 | 99 | sendpwm.send1(255, 1 , -0.3 * 100); |
hamohamo | 0:e32543d6e1e4 | 100 | while(LOOPdt.read_us()-t <= 10*1000/100*50); |
hamohamo | 0:e32543d6e1e4 | 101 | sendpwm.send1(255, 2 , 0.3 * 100);/* |
hamohamo | 0:e32543d6e1e4 | 102 | while(LOOPdt.read_us()-t <= 10*1000/100*70); |
hamohamo | 0:e32543d6e1e4 | 103 | sendpwm.send1(255, 3 , 0.3 * 100); |
hamohamo | 0:e32543d6e1e4 | 104 | while(LOOPdt.read_us()-t <= 10*1000/100*90); |
hamohamo | 0:e32543d6e1e4 | 105 | sendpwm.send1(255, 4 , 0.3 * 100);*/ |
hamohamo | 0:e32543d6e1e4 | 106 | while(LOOPdt.read_us()-t <= 10*1000); |
hamohamo | 0:e32543d6e1e4 | 107 | t = LOOPdt.read_us(); |
hamohamo | 0:e32543d6e1e4 | 108 | } |
hamohamo | 0:e32543d6e1e4 | 109 | |
hamohamo | 0:e32543d6e1e4 | 110 | } |
hamohamo | 0:e32543d6e1e4 | 111 | |
hamohamo | 0:e32543d6e1e4 | 112 | void route_read() { |
hamohamo | 0:e32543d6e1e4 | 113 | static double x, y, v, theta; |
hamohamo | 0:e32543d6e1e4 | 114 | wait(1); |
hamohamo | 0:e32543d6e1e4 | 115 | pc.printf("Hello World!\n"); |
hamohamo | 0:e32543d6e1e4 | 116 | wait(0.1); |
hamohamo | 0:e32543d6e1e4 | 117 | FILE *fp; |
hamohamo | 0:e32543d6e1e4 | 118 | do { |
hamohamo | 0:e32543d6e1e4 | 119 | fp = fopen("/sd/route.txt", "r"); |
hamohamo | 0:e32543d6e1e4 | 120 | wait(0.2); |
hamohamo | 0:e32543d6e1e4 | 121 | } while(fp==NULL); |
hamohamo | 0:e32543d6e1e4 | 122 | pc.printf("Good!\n"); |
hamohamo | 0:e32543d6e1e4 | 123 | while(fscanf(fp,"%lf %lf %lf %lf",&x, &y, &v, &theta) != EOF) { |
hamohamo | 0:e32543d6e1e4 | 124 | pc.printf("%lf %lf %lf %lf\n",x, y, v, theta); |
hamohamo | 0:e32543d6e1e4 | 125 | x_info.push_back(x); |
hamohamo | 0:e32543d6e1e4 | 126 | y_info.push_back(y); |
hamohamo | 0:e32543d6e1e4 | 127 | v_info.push_back(v); |
hamohamo | 0:e32543d6e1e4 | 128 | theta_info.push_back(theta); |
hamohamo | 0:e32543d6e1e4 | 129 | } |
hamohamo | 0:e32543d6e1e4 | 130 | pc.printf("checking...\n"); |
hamohamo | 0:e32543d6e1e4 | 131 | for(int k=0;k<x_info.size();k++){ |
hamohamo | 0:e32543d6e1e4 | 132 | pc.printf("%lf %lf %lf %lf\n",x_info[k],y_info[k],v_info[k],theta_info[k]); |
hamohamo | 0:e32543d6e1e4 | 133 | } |
hamohamo | 0:e32543d6e1e4 | 134 | fclose(fp); |
hamohamo | 0:e32543d6e1e4 | 135 | } |
hamohamo | 0:e32543d6e1e4 | 136 | |
hamohamo | 0:e32543d6e1e4 | 137 | void odmetry(){ |
hamohamo | 0:e32543d6e1e4 | 138 | |
hamohamo | 0:e32543d6e1e4 | 139 | static Matrix<double> Vr(3, 1); |
hamohamo | 0:e32543d6e1e4 | 140 | static Matrix<double> J(3, 4); |
hamohamo | 0:e32543d6e1e4 | 141 | static Matrix<double> W(4, 1); |
hamohamo | 0:e32543d6e1e4 | 142 | static double gyro = 0.0; |
hamohamo | 0:e32543d6e1e4 | 143 | static double x = 0.0; |
hamohamo | 0:e32543d6e1e4 | 144 | static double y = 0.0; |
hamohamo | 0:e32543d6e1e4 | 145 | static double vx = 0.0; |
hamohamo | 0:e32543d6e1e4 | 146 | static double vy = 0.0; |
hamohamo | 0:e32543d6e1e4 | 147 | static double sold0 = 0.0, sold1 = 0.0, sold2 = 0.0, sold3 = 0.0; |
hamohamo | 0:e32543d6e1e4 | 148 | static double cold0 = 0.0, cold1 = 0.0, cold2 = 0.0, cold3 = 0.0; |
hamohamo | 0:e32543d6e1e4 | 149 | double cor0 = 0.0, cor1 = 0.0, cor2 = 0.0, cor3 = 0.0; |
hamohamo | 0:e32543d6e1e4 | 150 | double side0 = 0.0, side1 = 0.0, side2 = 0.0, side3 = 0.0; |
hamohamo | 0:e32543d6e1e4 | 151 | static bool setup = false; |
hamohamo | 0:e32543d6e1e4 | 152 | if(!setup){ |
hamohamo | 0:e32543d6e1e4 | 153 | //bno.setmode(OPERATION_MODE_IMUPLUS); |
hamohamo | 0:e32543d6e1e4 | 154 | bno.reset(); |
hamohamo | 0:e32543d6e1e4 | 155 | bno.setmode(OPERATION_MODE_IMUPLUS); |
hamohamo | 0:e32543d6e1e4 | 156 | bno.get_angles(); |
hamohamo | 0:e32543d6e1e4 | 157 | /*while(fgyro - (bno.euler.yaw * (M_PI / 180.0)) != 0.0){ |
hamohamo | 0:e32543d6e1e4 | 158 | fgyro = bno.euler.yaw * (M_PI / 180.0); |
hamohamo | 0:e32543d6e1e4 | 159 | bno.get_angles(); |
hamohamo | 0:e32543d6e1e4 | 160 | }*/ |
hamohamo | 0:e32543d6e1e4 | 161 | setup = true; |
hamohamo | 0:e32543d6e1e4 | 162 | } |
hamohamo | 0:e32543d6e1e4 | 163 | |
hamohamo | 0:e32543d6e1e4 | 164 | cor0 = M_PI*cenc0.get()/1024.0; |
hamohamo | 0:e32543d6e1e4 | 165 | cor1 = M_PI*cenc1.get()/1024.0; |
hamohamo | 0:e32543d6e1e4 | 166 | cor2 = M_PI*cenc2.get()/1024.0; |
hamohamo | 0:e32543d6e1e4 | 167 | cor3 = M_PI*cenc3.get()/1024.0; |
hamohamo | 0:e32543d6e1e4 | 168 | cwv[0] = (cor0-cold0)/0.01; |
hamohamo | 0:e32543d6e1e4 | 169 | cwv[1] = (cor1-cold1)/0.01; |
hamohamo | 0:e32543d6e1e4 | 170 | cwv[2] = (cor2-cold2)/0.01; |
hamohamo | 0:e32543d6e1e4 | 171 | cwv[3] = (cor3-cold3)/0.01; |
hamohamo | 0:e32543d6e1e4 | 172 | side0 = M_PI*senc0.get()/1024.0; |
hamohamo | 0:e32543d6e1e4 | 173 | side1 = M_PI*senc1.get()/1024.0; |
hamohamo | 0:e32543d6e1e4 | 174 | side2 = M_PI*senc2.get()/1024.0; |
hamohamo | 0:e32543d6e1e4 | 175 | side3 = M_PI*senc3.get()/1024.0; |
hamohamo | 0:e32543d6e1e4 | 176 | swv[0] = (side0-sold0)/0.01; |
hamohamo | 0:e32543d6e1e4 | 177 | swv[1] = (side1-sold1)/0.01; |
hamohamo | 0:e32543d6e1e4 | 178 | swv[2] = (side2-sold2)/0.01; |
hamohamo | 0:e32543d6e1e4 | 179 | swv[3] = (side3-sold3)/0.01; |
hamohamo | 0:e32543d6e1e4 | 180 | |
hamohamo | 0:e32543d6e1e4 | 181 | cold0 = cor0; |
hamohamo | 0:e32543d6e1e4 | 182 | cold1 = cor1; |
hamohamo | 0:e32543d6e1e4 | 183 | cold2 = cor2; |
hamohamo | 0:e32543d6e1e4 | 184 | cold3 = cor3; |
hamohamo | 0:e32543d6e1e4 | 185 | |
hamohamo | 0:e32543d6e1e4 | 186 | sold0 = side0; |
hamohamo | 0:e32543d6e1e4 | 187 | sold1 = side1; |
hamohamo | 0:e32543d6e1e4 | 188 | sold2 = side2; |
hamohamo | 0:e32543d6e1e4 | 189 | sold3 = side3; |
hamohamo | 0:e32543d6e1e4 | 190 | |
hamohamo | 0:e32543d6e1e4 | 191 | W.set( 4, |
hamohamo | 0:e32543d6e1e4 | 192 | cwv[0], |
hamohamo | 0:e32543d6e1e4 | 193 | cwv[1], |
hamohamo | 0:e32543d6e1e4 | 194 | cwv[2], |
hamohamo | 0:e32543d6e1e4 | 195 | cwv[3] |
hamohamo | 0:e32543d6e1e4 | 196 | ); |
hamohamo | 0:e32543d6e1e4 | 197 | /* |
hamohamo | 0:e32543d6e1e4 | 198 | W.set( 4, |
hamohamo | 0:e32543d6e1e4 | 199 | swv[0], |
hamohamo | 0:e32543d6e1e4 | 200 | swv[1], |
hamohamo | 0:e32543d6e1e4 | 201 | swv[2], |
hamohamo | 0:e32543d6e1e4 | 202 | swv[3] |
hamohamo | 0:e32543d6e1e4 | 203 | );*/ |
hamohamo | 0:e32543d6e1e4 | 204 | |
hamohamo | 0:e32543d6e1e4 | 205 | //W.show(); |
hamohamo | 0:e32543d6e1e4 | 206 | bno.get_angles(); |
hamohamo | 0:e32543d6e1e4 | 207 | gyro = bno.euler.yaw * (M_PI / 180.0); |
hamohamo | 0:e32543d6e1e4 | 208 | theta = (2*M_PI-gyro); |
hamohamo | 0:e32543d6e1e4 | 209 | /* |
hamohamo | 0:e32543d6e1e4 | 210 | J.set( 12, |
hamohamo | 0:e32543d6e1e4 | 211 | -sin(-theta + M_PI/4.0), -sin(-theta + M_PI/2.0 + M_PI/4.0), -sin(-theta + M_PI + M_PI/4.0), -sin(-theta + 3.0*M_PI/2.0 + M_PI/4.0), |
hamohamo | 0:e32543d6e1e4 | 212 | -cos(-theta + M_PI/4.0), -cos(-theta + M_PI/2.0 + M_PI/4.0), -cos(-theta + M_PI + M_PI/4.0), -cos(-theta + 3.0*M_PI/2.0 + M_PI/4.0), |
hamohamo | 0:e32543d6e1e4 | 213 | 1.0/(2.0*R), 1.0/(2.0*R), 1.0/(2.0*R), 1.0/(2.0*R) |
hamohamo | 0:e32543d6e1e4 | 214 | ); |
hamohamo | 0:e32543d6e1e4 | 215 | */ |
hamohamo | 0:e32543d6e1e4 | 216 | |
hamohamo | 0:e32543d6e1e4 | 217 | J.set( 12, |
hamohamo | 0:e32543d6e1e4 | 218 | -sin(-theta - 2*M_PI/4), -sin(-theta + M_PI/2.0 - 2*M_PI/4), -sin(-theta + M_PI - 2*M_PI/4), -sin(-theta + 3.0*M_PI/2.0 - 2*M_PI/4), |
hamohamo | 0:e32543d6e1e4 | 219 | -cos(-theta - 2*M_PI/4), -cos(-theta + M_PI/2.0 - 2*M_PI/4), -cos(-theta + M_PI - 2*M_PI/4), -cos(-theta + 3.0*M_PI/2.0 - 2*M_PI/4), |
hamohamo | 0:e32543d6e1e4 | 220 | 1.0/(2.0*R), 1.0/(2.0*R), 1.0/(2.0*R), 1.0/(2.0*R) |
hamohamo | 0:e32543d6e1e4 | 221 | ); |
hamohamo | 0:e32543d6e1e4 | 222 | //J.show(); |
hamohamo | 0:e32543d6e1e4 | 223 | |
hamohamo | 0:e32543d6e1e4 | 224 | Vr = J*W*(r/2); |
hamohamo | 0:e32543d6e1e4 | 225 | //Vr.show(); |
hamohamo | 0:e32543d6e1e4 | 226 | |
hamohamo | 0:e32543d6e1e4 | 227 | x += ((vx + Vr[0][0]) * 0.01) / 2.0; |
hamohamo | 0:e32543d6e1e4 | 228 | y += ((vy + Vr[1][0]) * 0.01) / 2.0; |
hamohamo | 0:e32543d6e1e4 | 229 | vx = Vr[0][0]; |
hamohamo | 0:e32543d6e1e4 | 230 | vy = Vr[1][0]; |
hamohamo | 0:e32543d6e1e4 | 231 | c.x = x; |
hamohamo | 0:e32543d6e1e4 | 232 | c.y = y; |
hamohamo | 0:e32543d6e1e4 | 233 | pc.printf("x : %lf y : %lf vr : %lf theta : %lf\r\n", x, y, Vr[2][0], theta); |
hamohamo | 0:e32543d6e1e4 | 234 | //pc.printf("vx : %lf vy : %lf\r\n",Vr[0][0], Vr[1][0]); |
hamohamo | 0:e32543d6e1e4 | 235 | |
hamohamo | 0:e32543d6e1e4 | 236 | } |
hamohamo | 0:e32543d6e1e4 | 237 | |
hamohamo | 0:e32543d6e1e4 | 238 | void getpoint(pos *p,int n){ |
hamohamo | 0:e32543d6e1e4 | 239 | p->x = x_info[n]; |
hamohamo | 0:e32543d6e1e4 | 240 | p->y = y_info[n]; |
hamohamo | 0:e32543d6e1e4 | 241 | } |
hamohamo | 0:e32543d6e1e4 | 242 | |
hamohamo | 0:e32543d6e1e4 | 243 | void getvelocity(double *v,int n){ |
hamohamo | 0:e32543d6e1e4 | 244 | *v = v_info[n]; |
hamohamo | 0:e32543d6e1e4 | 245 | } |
hamohamo | 0:e32543d6e1e4 | 246 | |
hamohamo | 0:e32543d6e1e4 | 247 | void move(double vx,double vy,double vw){ |
hamohamo | 0:e32543d6e1e4 | 248 | |
hamohamo | 0:e32543d6e1e4 | 249 | static double k = sqrt(2.0)/2.0;; |
hamohamo | 0:e32543d6e1e4 | 250 | static double target[4]; |
hamohamo | 0:e32543d6e1e4 | 251 | |
hamohamo | 0:e32543d6e1e4 | 252 | target[0]= vx*k*(sin(theta)-cos(theta)) - vy*k*(sin(theta)+cos(theta)) + cR*vw; |
hamohamo | 0:e32543d6e1e4 | 253 | target[1]= -vx*k*(sin(theta)+cos(theta)) - vy*k*(sin(theta)-cos(theta)) + cR*vw; |
hamohamo | 0:e32543d6e1e4 | 254 | target[2]= -vx*k*(sin(theta)-cos(theta)) + vy*k*(sin(theta)+cos(theta)) + cR*vw; |
hamohamo | 0:e32543d6e1e4 | 255 | target[3]= vx*k*(sin(theta)+cos(theta)) + vy*k*(sin(theta)-cos(theta)) + cR*vw; |
hamohamo | 0:e32543d6e1e4 | 256 | |
hamohamo | 0:e32543d6e1e4 | 257 | /* |
hamohamo | 0:e32543d6e1e4 | 258 | target[1]=(k*vy-k*vx)+cR*vw; |
hamohamo | 0:e32543d6e1e4 | 259 | target[0]=(k*vx+k*vy)*-1; |
hamohamo | 0:e32543d6e1e4 | 260 | target[3]=(k*vx-k*vy)+cR*vw; |
hamohamo | 0:e32543d6e1e4 | 261 | target[2]=(k*vx+k*vy)+cR*vw;*/ |
hamohamo | 0:e32543d6e1e4 | 262 | mot0.SetParam(cwv[0]*r/100.0,target[0]/100.0); |
hamohamo | 0:e32543d6e1e4 | 263 | mot1.SetParam(cwv[1]*r/100.0,target[1]/100.0); |
hamohamo | 0:e32543d6e1e4 | 264 | mot2.SetParam(cwv[2]*r/100.0,target[2]/100.0); |
hamohamo | 0:e32543d6e1e4 | 265 | mot3.SetParam(cwv[3]*r/100.0,target[3]/100.0); |
hamohamo | 0:e32543d6e1e4 | 266 | |
hamohamo | 0:e32543d6e1e4 | 267 | //printf("%lf\n",mot0.GetGain(1.0)); |
hamohamo | 0:e32543d6e1e4 | 268 | //motor0.pwmout(mot0.GetGain(1.0) |
hamohamo | 0:e32543d6e1e4 | 269 | sendpwm.send1(255, 1 ,(mot0.GetGain(1.0) * 100)); |
hamohamo | 0:e32543d6e1e4 | 270 | while(LOOPdt.read_us()-t <= 10*1000/100*40); |
hamohamo | 0:e32543d6e1e4 | 271 | sendpwm.send1(255, 2 ,(mot1.GetGain(1.0) * 100)); |
hamohamo | 0:e32543d6e1e4 | 272 | while(LOOPdt.read_us()-t <= 10*1000/100*70); |
hamohamo | 0:e32543d6e1e4 | 273 | sendpwm.send1(255, 3 ,(mot2.GetGain(1.0) * 100)); |
hamohamo | 0:e32543d6e1e4 | 274 | while(LOOPdt.read_us()-t <= 10*1000/100*90); |
hamohamo | 0:e32543d6e1e4 | 275 | sendpwm.send1(255, 4 ,(mot3.GetGain(1.0) * 100)); |
hamohamo | 0:e32543d6e1e4 | 276 | //printf("%lf,%lf\n",cwv[0]*r,target[0]); |
hamohamo | 0:e32543d6e1e4 | 277 | } |