涼太郎 中村
/
2ndcomp
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: Locate Move Servo button mbed
Fork of
3servotest
by 
涼太郎 中村
Revision 4:1604d599d40f, committed 2016-09-02
- Comitter:
- sakanakuuun
- Date:
- Fri Sep 02 12:41:13 2016 +0000
- Parent:
- 3:56b034c41dc5
- Child:
- 5:97d9a34e5016
- Commit message:
- fs
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Locate.lib Fri Sep 02 12:41:13 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/sakanakuuun/code/Locate/#265d8ad19d2a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Move.lib Fri Sep 02 12:41:13 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/sakanakuuun/code/Move/#d7ff86f25eaa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/_hakaba/h_move.cpp Fri Sep 02 12:41:13 2016 +0000
@@ -0,0 +1,40 @@
+/*
+void pmovex2(int length)
+{
+ int px,py,ptheta,dx,dy,dtheta;
+ int k_y=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+ int k_theta=5;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+
+ update();
+ px=coordinateX();
+ py=coordinateY();
+ ptheta=coordinateTheta();
+ move(rightspeed,leftspeed);
+
+ while(1) {
+ update();
+
+ dx = coordinateX() - px;
+ dy = coordinateY() - py;
+ dtheta = coordinateTheta() - ptheta;
+
+ if(dy>7) {
+ dy=7;
+ }
+ else if(dy<-7) {
+ dy=-7;
+ }
+
+ if(-3 < dy && dy < 3){
+ move(rightspeed - k_y*dy - k_theta*dtheta, leftspeed + k_y*dy + k_theta*dtheta);
+ }
+ else{
+ move(rightspeed -50 - k_y*dy*dy - k_theta*dtheta, leftspeed -50 + k_y*dy*dy + k_theta*dtheta);
+ }
+ if(dx>length) {
+ move(0,0);
+ break;
+ }
+ }
+}
+*/
\ No newline at end of file
--- a/locate.h Fri Sep 02 06:50:14 2016 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,118 +0,0 @@
-#ifndef Locate_H
-#define Locate_H
-
-#include <math.h>
-#include"mbed.h"
-#include "Encoder.h"
-
-
-#define OUTERRING_D 140 //外輪間距離(mm)
-#define INNERRING_D 136 //内輪間距離(mm)
-#define PI 3.14159 //π
-#define RESOLUSION 400 //P/R(分解能)
-#define DIAMETER 31.8 //タイヤの直径(mm)
-#define LOCATE_STEP (DIAMETER*PI / RESOLUSION) // エンコーダの１ステップあたりの距離(mm)
-#define TIRE_DISTANCE ((OUTERRING_D + INNERRING_D) / 2) //タイヤ間距離(mm)
-#define ROUND_HOSEI 1 //角度のズレを補正
-#define ROUND ((PI * DIAMETER / (RESOLUSION * TIRE_DISTANCE)) * ROUND_HOSEI) //機体が１回転するために必要なステップ数の”逆数”
-
-//STM mbed bug: these macros are MISSING from stm32f3xx_hal_tim.h
-#ifdef TARGET_STM32F3
-#define __HAL_TIM_GET_COUNTER(__HANDLE__) ((__HANDLE__)->Instance->CNT)
-#define __HAL_TIM_IS_TIM_COUNTING_DOWN(__HANDLE__) (((__HANDLE__)->Instance->CR1 &(TIM_CR1_DIR)) == (TIM_CR1_DIR))
-#endif
-
-
-//エンコーダから、現在のステップ数（＝タイヤがどれだけ回ったか）を得られる
-
-
-//グローバル変数宣言
-Serial pc(SERIAL_TX, SERIAL_RX);
-TIM_Encoder_InitTypeDef encoder1, encoder2;
-TIM_HandleTypeDef timer1, timer2;
-DigitalOut enc_v(PC_7);
-
-uint16_t count1=0, count2=0;
-int8_t dir1, dir2;
-int r, l;
-int pr = 0, pl = 0; //前回のステップ数
-short v = 0; //ステップ速度
-float x = 0, y = 0; //xy方向に進んだ距離(m換算なし)
-float theta = 0; //機体角度、x軸正の向きを0とする
-float erase_theta = 0;
-//宣言終わり
-
-
-
-void setup() //エンコーダの初期のズレ(dr,dl)を出す、最初に一回だけ行う
-{
- enc_v = 1;
-
- //counting on both A&B inputs, 4 ticks per cycle, 16-bit count
- //use PB6 PB7 = Nucleo D7 D8
- EncoderInit(&encoder1, &timer1, TIM4, 0xffff, TIM_ENCODERMODE_TI12);
-
- //counting on both A&B inputs, 4 ticks per cycle, 16-bit count
- //use PA0 PA1 = Nucleo A0 A1
- EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI12);
-}
-
-int convert_enc_count(int16_t pulse, int8_t direction)
-{
- if(direction == 0)
- pulse = pulse - 0xffff -1;
-
- return pulse;
-}
-
-
-short coordinateX()
-//xをmm換算して整数値として返す
-{
- return x * LOCATE_STEP / 2;
-}
-
-short coordinateY()
-//yをmm換算して整数値として返す
-{
- return y * LOCATE_STEP / 2;
-}
-
-float coordinateTheta()
-//thetaを返す
-{
- return theta;
-}
-
-
-void update ()
-//位置情報を更新する。r,lはエンコーダから
-{
- count1=__HAL_TIM_GET_COUNTER(&timer1);
- dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1);
- count2=__HAL_TIM_GET_COUNTER(&timer2);
- dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);
-
- r = -convert_enc_count(count1, dir1);
- l = convert_enc_count(count2, dir2);
-
- theta = (r - l) * ROUND - erase_theta;
- v = (r - pr + l - pl);
-
- x += v * cos(theta);
- y += v * sin(theta);
-
- pr = r;
- pl = l;
- //pc.printf("count1:%d%s count2:%d%s\r\n", count1, dir1==0 ? "+":"-",count2, dir2==0 ? "+":"-");
- pc.printf("right:%d left:%d x:%d y:%d t:%f\n\r", r, l, coordinateX(), coordinateY(), coordinateTheta());
-}
-
-void erase()
-{
- x = 0;
- y = 0;
- erase_theta = theta;
-}
-
-#endif
--- a/main.cpp Fri Sep 02 06:50:14 2016 +0000
+++ b/main.cpp Fri Sep 02 12:41:13 2016 +0000
@@ -1,122 +1,23 @@
#include "mbed.h"
-#include "locate.h"
#include "math.h"
-PwmOut M1cw(PA_11);
-PwmOut M1ccw(PB_15);
-PwmOut M2ccw(PB_14);
-PwmOut M2cw(PB_13);
-
-const int allowlength=5;
-const float allowdegree=0.02;
-const int rightspeed=29*2;
-const int leftspeed=31*2;
-const int turnspeed=15*2;
-
-const float PIfact=2.89;
-
-void initmotor()
-{
-
-
- M1cw.period_us(256);
- M1ccw.period_us(256);
- M2cw.period_us(256);
- M2ccw.period_us(256);
-
-}
-
-void move(int left,int right)
-{
+#include "locate.h"
+#include "move.h"
- float rightduty,leftduty;
-
- if(right>256) {
- right=256;
- }
- if(left>256) {
- left=256;
- }
- if(right<-256) {
- right=-256;
- }
- if(left<-256) {
- left=-256;
- }
+int main()
+{
+ setup();
+ initmotor();
- rightduty=right/256.0;
- leftduty=left/256.0;
- if(right>0) {
- M1cw.write(1-rightduty);
- M1ccw.write(1);
- } else {
- M1cw.write(1);
- M1ccw.write(1+rightduty);
- }
+ turn_ccw();
- if(left>0) {
- M2cw.write(1-leftduty);
- M2ccw.write(1);
- } else {
- M2cw.write(1);
- M2ccw.write(1+leftduty);
+ while(1) {
+ update();
}
}
-void movelength(int length)
-{
- int px,py,pt;
- update();
- px=coordinateX();
- py=coordinateY();
- pt=coordinateTheta();
-
- move(rightspeed,leftspeed);
- while(1) {
-
- update();
- //pc.printf("dx:%d, dy:%d, l:%d x:%d y:%d t:%f\n\r",px-coordinateX(),py-coordinateY(),length,coordinateX(),coordinateY(), coordinateTheta());
- if(((px-coordinateX())*(px-coordinateX())+(py-coordinateY())*(py-coordinateY()))>length*length) {
- break;
- }
-
- }
- move(0,0);
-}
-
-void turncw()
-{
- float pt;
- move((-1)*turnspeed,turnspeed);
- update();
- pt=coordinateTheta();
- while(1) {
- update();
- if(pt-coordinateTheta()>PIfact/2) {
- move(0,0);
- break;
- }
- }
-}
-
-void turnccw()
-{
- float pt;
- move(turnspeed,(-1)*turnspeed);
-
- update();
- pt=coordinateTheta();
-
- while(1) {
- update();
- if(pt-coordinateTheta()<(-1)*PIfact/2) {
- move(0,0);
- break;
- }
- }
-}
/*
int main(){
setup();
@@ -127,36 +28,4 @@
movelength(1200);
turnccw();
}
-}*/
-
-void pmovex(int length){
- int px,py,dx,dy;
- int k=1;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
- update();
- px=coordinateX();
- py=coordinateY();
- move(rightspeed,leftspeed);
-
- while(1){
- update();
- dx=coordinateX()-px;
- dy=coordinateY()-py;
-
- if(dy>7){dy=7;}
- if(dy<-7){dy=-7;}
- move(rightspeed-k*dy,leftspeed+k*dy);
-
- if(dx>length){
- move(0,0);
- break;
- }
- }
-}
-
-int main()
-{
- setup();
- initmotor();
-
-
-}
\ No newline at end of file
+}*/
\ No newline at end of file
--- a/prime.h Fri Sep 02 06:50:14 2016 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,301 +0,0 @@
-
-/*
-primeではPINを制御する関数を扱う。
-
-以下一覧
-open,close hand
-open close arm
-
-step
-move
-blueorred
-lrsensor
-
-全関数共通で意図しない動作が起きたら基板上のLEDを点灯
-*/
-PwmOut pwmarm(PC_6);
-PwmOut pwmhand(PC_8);
-
-AnalogIn rightsensor(A0);
-AnalogIn leftsensor(A1);
-
-AnalogIn armadj(A2);
-AnalogIn handadj(A3);
-
-DigitalIn teamSW(PC_11);
-DigitalOut teamledblue(PC_10);
-DigitalOut teamledred(PC_12);
-
-DigitalOut errorled(LED1);
-
-
-DigitalIn phase1(PB_8);
-DigitalIn phase2(PC_9);
-DigitalIn phase4(PB_9);
-DigitalIn phase8(PD_2);
-
-PwmOut M1cw(PA_11);
-PwmOut M1ccw(PB_15);
-PwmOut M2ccw(PB_14);
-PwmOut M2cw(PB_13);
-
-DigitalOut PINW(PA_3);
-DigitalOut PINX(PA_2);
-DigitalOut PINY(PA_10);
-DigitalOut PINZ(PB_3);
-
-
-
-
-DigitalOut encordervcc1(PA_6);
-DigitalOut encordervcc2(PA_14);
-
-void initencorder(void){
- encordervcc1=1;
- encordervcc2=1;
- }
-
-
-
-void close_hand(void) {
- int i,degree;
-
- pwmhand.period_ms(20); //20ms
-
- degree=175;
-
- i=500+degree*1900/180;
- pwmhand.pulsewidth_us(i);
-
-
-}
-
-void close_arm(void) {
- PwmOut mypwm(PB_3);
- int i,degree;
-
- mypwm.period_ms(20); //20ms
-
- degree=160;
-
- i=500+degree*1900/180;
- pwmarm.pulsewidth_us(i);
-
-
-}
-
-
-
-void open_hand(void) {
- PwmOut mypwm(PWM_OUT);
- int i,degree;
-
- pwmhand.period_ms(20); //20ms
-
- degree=90;
-
- i=500+degree*1900/180;
- mypwm.pulsewidth_us(i);
-
-
-}
-
-
-
-void open_arm(void) {
- PwmOut mypwm(PWM_OUT);
- int i,degree;
-
- mypwm.period_ms(20); //20ms
-
- degree=10;
-
- i=500+degree*1900/180;
- pwmarm.pulsewidth_us(i);
-
-
-}
-
-void step(int degree){
-
-
-
- int puls_times=0;
-
- if(degree>0){
-
- puls_times=1+(int)(degree/(3.75));
-
- while(1){
- PINW=1;
- PINX=1;
- PINY=0;
- PINZ=0;
- wait_ms(20);
- puls_times--;
-
- if(puls_times<0){break;}
-
- PINW=0;
- PINX=1;
- PINY=1;
- PINZ=0;
- wait_ms(20);
- puls_times--;
-
- if(puls_times<0){break;}
-
- PINW=0;
- PINX=0;
- PINY=1;
- PINZ=1;
- wait_ms(20);
- puls_times--;
-
- if(puls_times<0){break;}
-
- PINW=1;
- PINX=1;
- PINY=0;
- PINZ=0;
- wait_ms(20);
- puls_times--;
-
- if(puls_times<0){break;}
- }
-
- }
-
- if(degree<0){
-
- puls_times=(-1)*(1+(int)(degree/(3.75)));
-
- while(1){
- PINW=1;
- PINX=1;
- PINY=0;
- PINZ=0;
- wait_ms(20);
- puls_times--;
-
- if(puls_times<0){break;}
-
- PINW=1;
- PINX=0;
- PINY=0;
- PINZ=1;
- wait_ms(20);
- puls_times--;
-
- if(puls_times<0){break;}
-
- PINW=0;
- PINX=0;
- PINY=1;
- PINZ=1;
- wait_ms(20);
- puls_times--;
-
- if(puls_times<0){break;}
-
- PINW=0;
- PINX=1;
- PINY=1;
- PINZ=0;
- wait_ms(20);
- puls_times--;
-
- if(puls_times<0){break;}
- }
-
- }
-
-}
-
-
-
-int sensor(void){
- //センサー読
- int right ,left,x,y;
-
- right=rightsensor.read();
- left=leftsensor.read();
-
- if(right>0.5){x=1;}
- else{right=0;}
-
- if(left>0.5){y=1;}
- else{left=0;}
-
- return x+2*(y);
- //(right,left)=(off,off),(on,off),(off,on),(on,on)
- // 0 1 2 3
-}
-
-
-
-void initmotor(){
-
-
- M1cw.period_us(256);
- M1ccw.period_us(256);
- M2cw.period_us(256);
- M2ccw.period_us(256);
-
-}
-
-void move(int right,int left){
-
- float rightduty,leftduty;
-
- if(right>256){right=256;}
- if(left>256){left=256;}
- if(right<-256){right=-256;}
- if(left<-256){left=-256;}
-
- rightduty=right/256.0;
- leftduty=left/256.0;
- if(right>0){
- M1cw.write(1-rightduty);
- M1ccw.write(1);
- }else{
- M1cw.write(1);
- M1ccw.write(1+rightduty);
- }
-
- if(left>0){
- M2cw.write(1-leftduty);
- M2ccw.write(1);
- }else{
- M2cw.write(1);
- M2ccw.write(1+leftduty);
- }
-}
-
-
-int phase(void){
-
- phase1.mode(PullUp);
- phase2.mode(PullUp);
- phase4.mode(PullUp);
- phase8.mode(PullUp);
-
- int r=0;
-
- r=phase1+2*phase2+4*phase4+8*phase8;
-
- return r;
-}
-
-
-void teamLED(){
- teamSW.mode(PullUp);
- if(teamSW){
- teamledblue=1;
- teamledred=0;
- }else{
- teamledblue=0;
- teamledred=1;
-
- }
-}
\ No newline at end of file