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@0:65eb28b8a9ae, 2015-02-16 (annotated)

Committer:
WAT34
Date:
Mon Feb 16 08:54:31 2015 +0000
Revision:
0:65eb28b8a9ae
omnicontroll; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WAT34 0:65eb28b8a9ae 1 #include "mbed.h"
WAT34 0:65eb28b8a9ae 2 #define pi 3.1415
WAT34 0:65eb28b8a9ae 3 AnalogIn stx(p20);
WAT34 0:65eb28b8a9ae 4 AnalogIn sty(p19);
WAT34 0:65eb28b8a9ae 5 DigitalIn sw1 (p11);
WAT34 0:65eb28b8a9ae 6 Serial pc(USBTX,USBRX);
WAT34 0:65eb28b8a9ae 7 BusOut m1(p5,p6);
WAT34 0:65eb28b8a9ae 8 BusOut m2(p7,p8);
WAT34 0:65eb28b8a9ae 9 BusOut m3(p9,p10);
WAT34 0:65eb28b8a9ae 10 PwmOut mp1(p21);
WAT34 0:65eb28b8a9ae 11 PwmOut mp2(p22);
WAT34 0:65eb28b8a9ae 12 PwmOut mp3(p23);
WAT34 0:65eb28b8a9ae 13 double get_theta(double stickx,double sticky)
WAT34 0:65eb28b8a9ae 14 {
WAT34 0:65eb28b8a9ae 15 double x,y,theta;
WAT34 0:65eb28b8a9ae 16 if (stickx>0.5){
WAT34 0:65eb28b8a9ae 17 x = stickx - 0.5;
WAT34 0:65eb28b8a9ae 18 }else if(stickx <= 0.5){
WAT34 0:65eb28b8a9ae 19 x = -(0.5 - stickx);
WAT34 0:65eb28b8a9ae 20 }else{
WAT34 0:65eb28b8a9ae 21 x = 0;
WAT34 0:65eb28b8a9ae 22 }
WAT34 0:65eb28b8a9ae 23 if (sticky>0.5){
WAT34 0:65eb28b8a9ae 24 y = sticky - 0.5;
WAT34 0:65eb28b8a9ae 25 }else if(sticky <= 0.5){
WAT34 0:65eb28b8a9ae 26 y = -(0.5 - sticky);
WAT34 0:65eb28b8a9ae 27 }else{
WAT34 0:65eb28b8a9ae 28 y = 0;
WAT34 0:65eb28b8a9ae 29 }
WAT34 0:65eb28b8a9ae 30 if(!(x==0 && y ==0)){
WAT34 0:65eb28b8a9ae 31 theta =atan(y/x);
WAT34 0:65eb28b8a9ae 32 }else {
WAT34 0:65eb28b8a9ae 33
WAT34 0:65eb28b8a9ae 34 theta = 0;
WAT34 0:65eb28b8a9ae 35 }
WAT34 0:65eb28b8a9ae 36 if(x>0 && y > 0){
WAT34 0:65eb28b8a9ae 37 theta = 3.141592+theta;
WAT34 0:65eb28b8a9ae 38 }else
WAT34 0:65eb28b8a9ae 39 if(x>0 && y < 0){
WAT34 0:65eb28b8a9ae 40 theta = pi+theta;
WAT34 0:65eb28b8a9ae 41 }else
WAT34 0:65eb28b8a9ae 42 if(x<0 && y < 0){
WAT34 0:65eb28b8a9ae 43 theta = theta;
WAT34 0:65eb28b8a9ae 44 }else{
WAT34 0:65eb28b8a9ae 45 theta = 2*pi+theta;
WAT34 0:65eb28b8a9ae 46 }
WAT34 0:65eb28b8a9ae 47 return theta;
WAT34 0:65eb28b8a9ae 48 }
WAT34 0:65eb28b8a9ae 49 void omni_cont(double theta)
WAT34 0:65eb28b8a9ae 50 {
WAT34 0:65eb28b8a9ae 51 double md1,md2,md3;
WAT34 0:65eb28b8a9ae 52 md1 = sin(theta-pi/3);
WAT34 0:65eb28b8a9ae 53 md2 = sin(theta-pi);
WAT34 0:65eb28b8a9ae 54 md3 = sin(theta-pi*5/3);
WAT34 0:65eb28b8a9ae 55 if (md1 < 0){
WAT34 0:65eb28b8a9ae 56 m1 = 2;
WAT34 0:65eb28b8a9ae 57 mp1 = -md1;
WAT34 0:65eb28b8a9ae 58 }else{
WAT34 0:65eb28b8a9ae 59 m1 = 1;
WAT34 0:65eb28b8a9ae 60 mp1 = md1;
WAT34 0:65eb28b8a9ae 61 }
WAT34 0:65eb28b8a9ae 62 if (md2 < 0){
WAT34 0:65eb28b8a9ae 63 m2 = 2;
WAT34 0:65eb28b8a9ae 64 mp2 = -md2;
WAT34 0:65eb28b8a9ae 65 }else{
WAT34 0:65eb28b8a9ae 66 m2 = 1;
WAT34 0:65eb28b8a9ae 67 mp2 = md2;
WAT34 0:65eb28b8a9ae 68 }
WAT34 0:65eb28b8a9ae 69 if (md3 < 0){
WAT34 0:65eb28b8a9ae 70 m3 = 2;
WAT34 0:65eb28b8a9ae 71 mp3 = -md3;
WAT34 0:65eb28b8a9ae 72 }else{
WAT34 0:65eb28b8a9ae 73 m3 = 1;
WAT34 0:65eb28b8a9ae 74 mp3 = md3;
WAT34 0:65eb28b8a9ae 75 }
WAT34 0:65eb28b8a9ae 76 }
WAT34 0:65eb28b8a9ae 77 int main() {
WAT34 0:65eb28b8a9ae 78 sw1.mode(PullUp);
WAT34 0:65eb28b8a9ae 79 double theta;
WAT34 0:65eb28b8a9ae 80 while(1) {
WAT34 0:65eb28b8a9ae 81 theta = get_theta(stx,sty);
WAT34 0:65eb28b8a9ae 82 if(sw1 == 0){
WAT34 0:65eb28b8a9ae 83 omni_cont(theta);
WAT34 0:65eb28b8a9ae 84 }else{
WAT34 0:65eb28b8a9ae 85 m1 = 0;
WAT34 0:65eb28b8a9ae 86 m2 = 0;
WAT34 0:65eb28b8a9ae 87 m3 = 0;
WAT34 0:65eb28b8a9ae 88 }
WAT34 0:65eb28b8a9ae 89 }
WAT34 0:65eb28b8a9ae 90 }