Akashi_2021_Bteam / Mbed 2 deprecated AKASHIKOSEN

sdf

Dependencies:   mbed arrc_mbed BNO055 SDFileSystem NK_mbed

main.cpp@1:32030d9ba873, 2021-08-22 (annotated)

Committer:
hamohamo
Date:
Sun Aug 22 13:07:29 2021 +0000
Revision:
1:32030d9ba873
Parent:
0:e32543d6e1e4 
asd

Who changed what in which revision?

UserRevisionLine numberNew 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 }