nakashima Kohei / Mbed 2 deprecated SHIPBOT_LATEST

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed arrc_mbed BNO055 take_mbed_old SDFileSystem ros_lib_melodic_buffer_increase

Files at this revision

API Documentation at this revision

Revision 0:f80bb69e638e, committed 2021-10-13

Comitter:
hamohamo
Date:
Wed Oct 13 08:47:48 2021 +0000
Child:
1:aa9cc4250220
Commit message:
ROBOKON BTEAM SHIPBOT

Changed in this revision

BNO055.lib Show annotated file Show diff for this revision Revisions of this file
SDFileSystem.lib Show annotated file Show diff for this revision Revisions of this file
arrc_mbed.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
take_mbed.lib Show annotated file Show diff for this revision Revisions of this file 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BNO055.lib	Wed Oct 13 08:47:48 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/StressedDave/code/BNO055/#1f722ffec323

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.lib	Wed Oct 13 08:47:48 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/kenjiArai/code/SDFileSystem/#624b059495aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/arrc_mbed.lib	Wed Oct 13 08:47:48 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/TanakaRobo/code/arrc_mbed/#77c13e86ad12

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 13 08:47:48 2021 +0000
@@ -0,0 +1,206 @@
+#define USESCRP
+#include "core.hpp"
+#include "SDFileSystem.h"
+#include "BNO055.h"
+#include <cmath>
+#include <vector>
+
+#include "mbed.h"
+#define POINTS 900
+
+
+/*--------------FUNCTION--------------*/
+void init();
+void route_read();
+void getPoint(Position *p,int n);
+void getVelocity(double *v,int n);
+bool incircle(Position pos,Position target,double error);
+void gyroA(double theta);
+void gyroB(double theta);
+bool checkMOT12(int rx_data, int &tx_data);
+bool checkMOT34(int rx_data, int &tx_data);
+
+
+/*--------------VARIABLE--------------*/
+
+std::vector<double> t_info,x_info,y_info,v_info,theta_info;
+Position pos,NextP,subP;
+int n=1;
+double v=0.0;
+double theta = 0.0;
+double gyro = 0.0;
+bool gyro_1 = false,gyro_2 = false;
+long long g_pulse;
+bool MOT12 = false,MOT34 = false;
+
+BNO055 bno(PB_3, PB_10);
+
+SDFileSystem sd(PB_15, PB_14, PB_13, PB_1,"sd");// mosi, miso, sclk, cs
+ScrpSlave scrp(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800);
+Robot AKASHIKOSEN(50.8,25.4,322.5,259.75);
+Core RBT(&AKASHIKOSEN,&scrp,OMNI4,0.02);
+
+int main(){
+
+/*--------------SETUP--------------*/
+scrp.addCMD(6,checkMOT12);
+scrp.addCMD(7,checkMOT34);
+AKASHIKOSEN.setCWID(1,2,3,4);
+AKASHIKOSEN.setSWID(5,6,7,8);
+    
+RBT.addENC(PC_4,PA_13,512,4,1);
+RBT.addENC(PA_14,PA_15,512,4,2);
+RBT.addENC(PC_2,PC_3,512,4,3);
+RBT.addENC(PC_10,PC_11,512,4,4);
+RBT.addENC(PA_7,PA_6,512,4,5);
+RBT.addENC(PA_9,PA_8,512,4,6);
+RBT.addENC(PC_1,PC_0,512,4,7);
+RBT.addENC(PC_5,PA_12,512,4,8);
+
+RBT.addPID(0.0048,0.001,0.00008,1)->setLimit(0.5,-0.5);
+RBT.addPID(0.0048,0.001,0.00008,2)->setLimit(0.5,-0.5);
+RBT.addPID(0.0048,0.001,0.00008,3)->setLimit(0.5,-0.5);
+RBT.addPID(0.0048,0.001,0.00008,4)->setLimit(0.5,-0.5);
+RBT.addPID(0.5,0.001,0.2,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0);
+
+init();
+
+route_read();
+
+wait(1);
+
+
+bool setup = false;
+if(!setup){
+    bno.reset();
+    bno.setmode(OPERATION_MODE_IMUPLUS);
+    setup = true;
+}
+    
+RBT.START();
+/*--------------LOOP--------------*/
+while(true){
+    pos = RBT.getStatus();
+    
+    bno.get_angles();
+    gyro = 2.0*M_PI - bno.euler.yaw * (M_PI / 180.0);
+    gyroA(gyro);
+    gyroB(gyro);
+    
+    theta = 2.0*M_PI*(int)(g_pulse/4.0);
+    if(g_pulse > 0.0) theta += gyro;
+    else if(g_pulse < 0.0) theta += gyro-2.0*M_PI;
+    else if(g_pulse == 0 and gyro < M_PI/2.0) theta += gyro;
+    else if(g_pulse == 0 and gyro > 3.0*M_PI/2.0) theta += gyro-2.0*M_PI;
+    
+    RBT.setPosition(pos.x,pos.y,theta);
+    
+    //RBT.setVelocity(0.0,0.0,0.0);
+    if(n<POINTS and 
+        ( (incircle(pos,NextP,30.0) and n < POINTS and n != 1000) or 
+            ( incircle(pos,NextP,10.0) and n == 1000 ) or
+            ( sqrt( (NextP.x-pos.x)*(NextP.x-pos.x)+(NextP.y-pos.y)*(NextP.y-pos.y) ) > sqrt( (subP.x-pos.x)*(subP.x-pos.x)+(subP.y-pos.y)*(subP.y-pos.y) ) and n < POINTS)
+        ) 
+    )
+    {
+        n++;
+        getPoint(&NextP,n);
+        getVelocity(&v,n-1);
+        if(n < POINTS)getPoint(&subP,n+1);
+        if(v > 0.0) v *= 1.7;
+        if(v >= 1400) v = 1400;
+        //v = 200.0;
+        if(n == POINTS) v = 50.0;
+        printf("%d:pass\n",n);
+    }
+    if(n == POINTS and incircle(pos,NextP,1.0)) v = 0.0;
+    double etheta = atan2(NextP.y-pos.y,NextP.x-pos.x);
+    RBT.PIDs[5]->Update(pos.theta,NextP.theta,0.02);
+    RBT.sendVelocity(v*cos(etheta),v*sin(etheta),RBT.PIDs[5]->getGain());
+    //RBT.sendVelocity(200.0,200.0,0.0);
+    printf("x:%lf,y:%lf,theta:%lf,n:%d\n",pos.x,pos.y,pos.theta,n);
+    RBT.LOOP();
+}
+}
+
+void route_read() {
+    static double x, y, v, theta_;
+    wait(1);
+    printf("Hello World!\n");
+    wait(0.1);
+    FILE *fp;
+    do {
+        fp = fopen("/sd/route.txt", "r");
+        wait(0.2);
+    } while(fp==NULL);
+    printf("Good!\n");
+    while(fscanf(fp,"%lf %lf %lf %lf",&x, &y, &v, &theta_) != EOF) {
+        printf("%lf %lf %lf %lf\n",x, y, v, theta_);
+        x_info.push_back(x);
+        y_info.push_back(y);
+        v_info.push_back(v);
+        theta_info.push_back(theta_);
+    }
+    printf("checking...\n");
+    for(int k=0;k<x_info.size();k++){
+        printf("%lf %lf %lf %lf\n",x_info[k],y_info[k],v_info[k],theta_info[k]);
+    }
+    fclose(fp);
+}
+
+void getPoint(Position *p,int n){
+    p->x = x_info[n];
+    p->y = y_info[n];
+    p->theta = theta_info[n];
+}
+
+void getVelocity(double *v,int n){
+    *v = v_info[n];
+}
+
+bool incircle(Position pos,Position target,double error){
+    return (sqrt( (target.x-pos.x)*(target.x-pos.x)+(target.y-pos.y)*(target.y-pos.y) ) < error);
+}
+void gyroA(double theta){
+    static bool prev = false;
+    if(0 <= theta and theta < M_PI) gyro_1 = false;
+    else gyro_1 = true;
+    if(gyro_1 and !prev){
+        if(gyro_2) g_pulse++;
+        else if(g_pulse != 0) g_pulse--;
+    }
+    else if(!gyro_1 and prev){
+        if(gyro_2) g_pulse--;
+        else if(g_pulse != 0)g_pulse++;
+    }
+    prev = gyro_1;
+}
+void gyroB(double theta){
+    static bool prev = false;
+    if((3.0*M_PI/2 <= theta and theta <= 2*M_PI) or (theta < M_PI/2.0)) gyro_2 = false;
+    else gyro_2 = true;
+    if(gyro_2 and !prev){
+        if(gyro_1) g_pulse--;
+        else g_pulse++;
+    }
+    else if(!gyro_2 and prev){
+        if(gyro_1) g_pulse++;
+        else g_pulse--;
+    }
+    prev = gyro_2;
+}
+
+bool checkMOT12(int rx_data, int &tx_data){
+    MOT12 = true;
+    return true;
+}
+bool checkMOT34(int rx_data, int &tx_data){
+    MOT34 = true;
+    return true;
+}
+
+void init(){
+    MOT12 = false,MOT34 = false;
+    scrp.send1(255,5,1);
+    while(!MOT12 or !MOT34);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Oct 13 08:47:48 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/take_mbed.lib	Wed Oct 13 08:47:48 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/hamohamo/code/take_mbed/#3f42230ca4ec