mbedを用いた制御学生の制御 / Mbed 2 deprecated omni_stick

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

main.cpp

Committer:
WAT34
Date:
2015-02-16
Revision:
0:65eb28b8a9ae

File content as of revision 0:65eb28b8a9ae:

#include "mbed.h"
#define pi 3.1415
AnalogIn stx(p20);
AnalogIn sty(p19);
DigitalIn sw1 (p11);
Serial pc(USBTX,USBRX);
BusOut m1(p5,p6);
BusOut m2(p7,p8);
BusOut m3(p9,p10);
PwmOut mp1(p21);
PwmOut mp2(p22);
PwmOut mp3(p23);
double get_theta(double stickx,double sticky)
{
    double x,y,theta;
    if (stickx>0.5){
        x = stickx - 0.5;
    }else if(stickx <= 0.5){
        x = -(0.5 - stickx);
    }else{
        x = 0;
    }
    if (sticky>0.5){
        y = sticky - 0.5;
    }else if(sticky <= 0.5){
        y = -(0.5 - sticky);
    }else{
        y = 0;
    }
    if(!(x==0 && y ==0)){
        theta =atan(y/x);
    }else {
            
       theta = 0;
    }
    if(x>0 && y > 0){
        theta = 3.141592+theta;
    }else
    if(x>0 && y < 0){
       theta = pi+theta;
    }else
    if(x<0 && y < 0){
        theta = theta;
    }else{
     theta = 2*pi+theta;
    }
    return theta;
}
void omni_cont(double theta)
{
    double md1,md2,md3;
    md1 = sin(theta-pi/3);
    md2 = sin(theta-pi);
    md3 = sin(theta-pi*5/3);
    if (md1 < 0){
        m1 = 2;
        mp1 = -md1;
    }else{
        m1 = 1;
        mp1 = md1;
    } 
    if (md2 < 0){
        m2 = 2;
        mp2 = -md2;
    }else{
        m2 = 1;
        mp2 = md2;
    } 
    if (md3 < 0){
        m3 = 2;
        mp3 = -md3;
    }else{
        m3 = 1;
        mp3 = md3;
    }
} 
int main() {
    sw1.mode(PullUp);
    double theta;   
    while(1) {
        theta = get_theta(stx,sty);
        if(sw1 == 0){
            omni_cont(theta);
        }else{
            m1 = 0;
            m2 = 0;
            m3 = 0;
        }
    }
}