CatPot 2015-2016
/
CatPot_Main_ver1
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: AQM0802A L6470_lib PID mbed
Revision 1:7d4921b5d638, committed 2014-12-16
- Comitter:
- ryuna
- Date:
- Tue Dec 16 09:13:08 2014 +0000
- Parent:
- 0:ae08e2e1d82d
- Child:
- 2:054444aa1990
- Commit message:
- update_?????????????????MultiSerial???
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/L6470_lib.lib Tue Dec 16 09:13:08 2014 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/2014/code/L6470_lib/#f2854fe95fdd
--- a/L6470_lib/L6470.cpp Tue Nov 11 09:10:40 2014 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,296 +0,0 @@
-/*
- *mbed L6470 SPI Library
- */
-
-#include "mbed.h"
-#include "L6470.h"
-
-
- L6470::L6470(PinName mosi, PinName miso, PinName sclk, PinName cs,PinName busy)
- : m_spi(mosi, miso, sclk), m_cs(cs),m_busy(busy,PullUp)
- {
- //定義時に行う
- m_cs = 1;
- m_spi.format(8,3);
- m_spi.frequency(1000000);
-
- /*///前回の送りデータを流す
- send(0x00);
- send(0x00);
- send(0x00);
- send(0x00);
- /**/
-
- }
- void L6470::send(uint8_t temp)
- {
- //while(!m_busy){
- //}
- m_cs = 0;//#cs = 1;
- temp = m_spi.write((uint8_t)temp);
- m_cs = 1;//#cs = 0;
-
- wait_us(0.5);
-
- }
-
-
- uint8_t L6470::send_r(uint8_t temp)
- {
- //while(!m_busy){
- //}
- m_cs = 0;//#cs = 1;
- temp = m_spi.write((uint8_t)temp);
- m_cs = 1;//#cs = 0;
-
- wait_us(1);
- return temp;
- }
-
- void L6470::send_bytes(uint8_t temp[], int i)
- {
- while(0< i--)
- {
- send(temp[i]);
- }
- }
-
- void L6470::send_bytes_r(uint8_t temp[], int i)
- {
- while(0< i--)
- {
- temp[i] = send_r(temp[i]);
- }
- }
-
-
-
-
- void L6470::NOP()
- {
- send(0x00);
-
- }
-
- void L6470::SetParam(int param, int value)
- {
- int n = (param>>8)/8;//配列の数
- int m = (param>>8)%8;//nは割り算だからあまりの分の値が取れないためこちらをif文に活用
- if(!m){
- uint8_t temp[n+1];
- temp[n] = 0x00 | (uint8_t)(param & 0xFF);
- while(0 < n--){//比較後マイナス
- temp[n] = (uint8_t)(value >> 8*n )&0xFF;
- }
- send_bytes(temp,sizeof temp/sizeof temp[0]);
-
- }else{//余りが出た場合. 例 22とか7
- uint8_t temp[n+2];
- temp[n+1] = 0x00 | (uint8_t)(param & 0xFF);
- temp[n] = (uint8_t)(value >> 8*n)&~(0xFF<<m);//FFをずらしたあとで1の補数変換で反転.
- while(0 < n--){
- temp[n]= (uint8_t)(value >> 8*n)&0xFF;
- }
- send_bytes(temp,sizeof temp/sizeof temp[0]);
- }
-
- }
- int L6470::GetParam(int param)
- {
- int value = 0;
- int n = (param>>8)/8;//配列の数
- int m = (param>>8)%8;//nは割り算だからあまりの分の値が取れないためこちらをif文に活用
-
- if(!m){
- uint8_t temp[n+1];
- for(int i = 0; i < n+1; i++){
- temp[i] = 0;
- }
- temp[n] = 0x20|(uint8_t)(param&0xFF);
- send_bytes_r(temp,sizeof temp/sizeof temp[0]);
- while(0 < n--){
- value |= (int)temp[n] << 8*n;
- }
-
- }else{
- n++;
- uint8_t temp[n+1];
- for(int i = 0; i < n+2; i++){
- temp[i] = 0;
- }
- temp[n] = 0x20|(uint8_t)(param&0xFF);
- send_bytes_r(temp,sizeof temp/sizeof temp[0]);
- while(0 < n--){
- value |= (int)temp[n]<< 8*n;
- }
-
- }
-
- return value;
- }
-
-
-
- void L6470::Run(bool dir, int speed)
- {
- uint8_t temp[4];
- temp[3] = 0x50|dir;
- temp[2] = (uint8_t)(speed >> 16)&0x0F;
- temp[1] = (uint8_t)(speed >> 8)&0xFF;
- temp[0] = (uint8_t)(speed >> 0)&0xFF;
- send_bytes(temp,sizeof temp/sizeof temp[0]);
-
- }
-
- void L6470::StepClock(bool dir)
- {
- send(0x58|dir);
- }
-
- void L6470::Move(bool dir,int n_step )
- {
- uint8_t temp[4];
- temp[3] = 0x40|dir;
- temp[2] = (uint8_t)(n_step >> 16)&0x3F;
- temp[1] = (uint8_t)(n_step >> 8)&0xFF;
- temp[0] = (uint8_t)(n_step >> 0)&0xFF;
- send_bytes(temp,sizeof temp/sizeof temp[0]);
-
- }
- void L6470::GoTo(int abs_pos)
- {
- uint8_t temp[4];
- temp[3] = 0x60;
- temp[2] = (uint8_t)(abs_pos >> 16)&0x3F;
- temp[1] = (uint8_t)(abs_pos >> 8)&0xFF;
- temp[0] = (uint8_t)(abs_pos >> 0)&0xFF;
- send_bytes(temp,sizeof temp/sizeof temp[0]);
-
- }
-
- void L6470::GoTo_dir(bool dir, int abs_pos)
- {
- uint8_t temp[4];
- temp[3] = 0x68|dir;
- temp[2] = (uint8_t)(abs_pos >> 16)&0x3F;
- temp[1] = (uint8_t)(abs_pos >> 8)&0xFF;
- temp[0] = (uint8_t)(abs_pos >> 0)&0xFF;
- send_bytes(temp,sizeof temp/sizeof temp[0]);
-
- }
-
- void L6470::GoUntill(bool act, bool dir, int speed)
- {
- uint8_t temp[4];
- temp[3] = 0x82|(act<<8)|dir;
- temp[2] = (uint8_t)(speed >> 16)&0x3F;
- temp[1] = (uint8_t)(speed >> 8)&0xFF;
- temp[0] = (uint8_t)(speed >> 0)&0xFF;
- send_bytes(temp,sizeof temp/sizeof temp[0]);
- }
-
- void L6470::ReleseSW(bool act,bool dir)
- {
- send(0x92|(act <<3)|dir);
- }
- void L6470::GoHome()
- {
- send(0x70);
- }
-
- void L6470::GoMark()
- {
- send(0x78);
- }
-
- void L6470::ResetPos()
- {
- send(0xD8);
- }
-
- void L6470::ResetDevice()
- {
- send(0xC0);
- }
-
- void L6470::SoftStop()
- {
- send(0xB0);
- }
-
- void L6470::HardStop()
- {
- send(0xB8);
- }
-
- void L6470::SoftHiz()
- {
- send(0xA0);
- }
-
- void L6470::HardHiz()
- {
- send(0xA8);
- }
-
- void L6470::Resets()
- {
- SoftStop();
- wait(0.5);
- ResetDevice();
- SetParam(ABS_POS,INI_ABS_POS);
- SetParam(EL_POS,INI_EL_POS);
- SetParam(MARK,INI_MARK);
- SetParam(SPEED,INI_SPEED);
- SetParam(ACC,INI_ACC);
- SetParam(DEC,INI_DEC);
- SetParam(MAX_SPEED,INI_MAX_SPEED);
- SetParam(MIN_SPEED,INI_MIN_SPEED);
- SetParam(KVAL_HOLD,INI_KVAL_HOLD);
- SetParam(KVAL_RUN,INI_KVAL_RUN);
- SetParam(KVAL_ACC,INI_KVAL_ACC);
- SetParam(KVAL_DEC,INI_KVAL_DEC);
- SetParam(INT_SPD,INI_INT_SPD);
- SetParam(ST_SLP,INI_ST_SLP);
- SetParam(FN_SLP_ACC,INI_FN_SLP_ACC);
- SetParam(FN_SLP_DEC,INI_FN_SLP_DEC);
- SetParam(K_THERM,INI_K_THERM);
- SetParam(OCD_TH,INI_OCD_TH);
- SetParam(STALL_TH,INI_STALL_TH);
- SetParam(FS_SPD,INI_FS_SPD);
- SetParam(STEP_MODE,INI_STEP_MODE);
- SetParam(ALARM_EN,INI_ALARM_EN);
- SetParam(CONFIG,INI_CONFIG);
-
- }
-
- void L6470::BusyWait(unsigned int time)
- {//BESYが解除されるまで待機
- while(!m_busy){
- }
-
- wait_ms(time);
- }
-
- void L6470::Step(int dosu)
- {
- //初期値180度とする
- int temp;
-
- temp = dosu*200/360;
-
- GoTo(temp);
-
-
-
- }
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
--- a/L6470_lib/L6470.h Tue Nov 11 09:10:40 2014 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,134 +0,0 @@
-
-#ifndef L6470_STEP_H
-#define L6470_STEP_H
-
-//Brid. respected
-#include "mbed.h"
-
-//config name.reset data;
-//first init config data.
-#define INI_ABS_POS 0
-#define INI_EL_POS 0
-#define INI_MARK 180
-#define INI_SPEED 0
-#define INI_ACC 0x8A//8A//0010
-#define INI_DEC 0x8A
-#define INI_MAX_SPEED 0x20
-#define INI_MIN_SPEED 0
-#define INI_FS_SPD 0x27
-#define INI_KVAL_HOLD 0x3F
-#define INI_KVAL_RUN 0xFF
-#define INI_KVAL_ACC 0x70
-#define INI_KVAL_DEC 0x70
-#define INI_INT_SPD 0x408
-#define INI_ST_SLP 0x19
-#define INI_FN_SLP_ACC 0x29
-#define INI_FN_SLP_DEC 0x29
-#define INI_K_THERM 0
-// INI_ADC_OUT read only desu.
-#define INI_OCD_TH 0xF0
-#define INI_STALL_TH 0x7F
-#define INI_STEP_MODE 0x70
-#define INI_ALARM_EN 0xFF
-#define INI_CONFIG 0x2E88
-// INI_STATUS read only desu.
-
-
-
-
-//レジスタのアドレス
-// name length<<8 + address
-#define ABS_POS ((22<<8)+0x01)
-#define EL_POS ((9<<8)+0x02)
-#define MARK ((22<<8)+0x03)
-#define SPEED ((20<<8)+0x04)
-#define ACC ((12<<8)+0x05)
-#define DEC ((12<<8)+0x06)
-#define MAX_SPEED ((10<<8)+0x07)
-#define MIN_SPEED ((13<<8)+0x08)
-#define FS_SPD ((10<<8)+0x15)
-#define KVAL_HOLD ((8<<8)+0x09)
-#define KVAL_RUN ((8<<8)+0x0A)
-#define KVAL_ACC ((8<<8)+0x0B)
-#define KVAL_DEC ((8<<8)+0x0C)
-#define INT_SPD ((14<<8)+0x0D)
-#define ST_SLP ((8<<8)+0x0E)
-#define FN_SLP_ACC ((8<<8)+0x0F)
-#define FN_SLP_DEC ((8<<8)+0x10)
-#define K_THERM ((4<<8)+0x11)
-#define ADC_OUT ((5<<8)+0x12)
-#define OCD_TH ((4<<8)+0x13)
-#define STALL_TH ((7<<8)+0x14)
-#define STEP_MODE ((8<<8)+0x16)
-#define ALARM_EN ((8<<8)+0x17)
-#define CONFIG ((16<<8)+0x18)
-#define STATUS ((16<<8)+0x19)
-
-class L6470{
-
- public:
-
- /**
- *
- * @param mosi SPI mosi pin
- * @param miso SPI miso pin
- * @param sck SPI sck pin
- * @param _cs #cs pin
- *
- */
-
- L6470(PinName mosi, PinName miso, PinName sck, PinName cs, PinName busy);
-
- void send(uint8_t temp);
- uint8_t send_r(uint8_t temp);
- void send_bytes(uint8_t temp[], int i);
- void send_bytes_r(uint8_t temp[], int i);
- void NOP();
- void SetParam(int param, int value);
- int GetParam(int param);
- void Run(bool dir, int speed);
- void StepClock(bool dir);
- void Move(bool dir,int n_step );//n_step--22bit
- void GoTo(int abs_pos);
- void GoTo_dir(bool dir, int abs_pos);
- void GoUntill(bool act, bool dir, int speed);
- void ReleseSW(bool act,bool dir);
- void GoHome();
- void GoMark();
- void ResetPos();
- void ResetDevice();
- void SoftStop();
- void HardStop();
- void SoftHiz();
- void HardHiz();
- void Resets();
- void BusyWait(unsigned int time);
- void Step(int dosu);
-
-
-
- protected:
-
- SPI m_spi;
- DigitalOut m_cs;
- DigitalIn m_busy;
-
-
-
-};
-#endif
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
--- a/MultiSerial.lib Tue Nov 11 09:10:40 2014 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/teams/B/code/MultiSerial/#7620ef9e7187
--- a/main.cpp Tue Nov 11 09:10:40 2014 +0000
+++ b/main.cpp Tue Dec 16 09:13:08 2014 +0000
@@ -7,20 +7,21 @@
*
* データからロボットの移動やキッカー等のモータの動作を決定する処理を行う
*
- * MotorDriverにmaxonとかキッカーモータに命令
+ * MotorDriverにmaxonに命令
*
* SteppingMotorDriverにステアリング命令
*
* LCDでデバック
*
* スイッチ4つとスタートスイッチで処理を実行
- *
+ *
+ * キッカーのスイッチがどこかに入る
*
*************************
*
*Pin Map
*
- * p5,p6,p7,p29,p30? >> SPI >>Stepping
+ * p5,p6,p7,p29,p30 >> SPI >>Stepping
*
* p9,p10 >> I2C orSerial >> LPC1114FN28/102 read
*
@@ -36,21 +37,20 @@
#include "mbed.h"
#include "L6470.h"
-#include "MultiSerial.h"
#include "PID.h"
#include "AQM0802A.h"
#include <math.h>
#include <sstream>
-ACM0802A Lcd(p27,p28);
-L6470 Step(p5,p6,p7,p29,p30);
-I2C Sensor(p9,p10);
-Serial Motor(p13,p14);
-Serila pc(USBTX,USBRX);
+DigitalIn DebugSw[4] = {p22,p23,p24,p25};
+DigitalIn StartSw(p26,PullUp);
+DigitalOut Led[4] = {LED1,LED2,LED3,LED4};
-
-DigitalOut Led[4] = {LED1,LED2,LED3,LEd4};
-
+I2C Sensor(p9,p10);//sda,scl
+AQM0802A Lcd(p28,p27);//sda,scl
+L6470 Step(p5,p6,p7,p29,p30);//mosi,miso,sck,#cs,busy(PullUp)
+Serial Motor(p13,p14);//tx,rx
+Serial pc(USBTX,USBRX);
extern string StringFIN;
@@ -60,7 +60,7 @@
int speed[4] = {0};
-void move(double vx, double vy, int vs, int vk){
+void move(int vx, int vy, int vs, int vk){
double pwm[4] = {0};
uint8_t i = 0;
pwm[0] = (double)((vx) + vs);
@@ -78,14 +78,22 @@
}
}
+//通信（モータ用）
+void tx_motor(){
+ array(speed[0],speed[1],speed[3],speed[2]);
+ Motor.printf("%s",StringFIN.c_str());
+}
int main() {
+ Motor.baud(115200); //ボーレート設定
+ Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
+ Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
+ move(0,0,0,0);
while(1) {
- myled = 1;
- wait(0.2);
- myled = 0;
- wait(0.2);
+ move(30,30,0,0);
+ wait(1.5);
+
}
}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/wraparound.cpp Tue Dec 16 09:13:08 2014 +0000 @@ -0,0 +1,58 @@ +//回り込みます +/** +ballの位置によりボールが正面に来るまで．を一連の処理動作とし，動作を考える． +すなわち，逐一モーターを動かし，ボールとの位置をチェックしモーターを動かすのではなく，はじめの状態から移動を考えてその処理を終えるまでモーターは拘束される． + +この動作について信用を得るために超音波(後ろ，左右)，irセンサ，地磁気センサの三つから現在状態およびボールとの距離を把握．計算により移動量，回転速度を決定，行動へ． + + + +上位4ビットで一番目のボールの位置，下位4ビットで二番目のボールの位置 +二つ目の8ビットで1個目のボールの値 +三つ目の8ビットで2個目のボールの値 + +Ir1//irの一番値が大きいもの +Ir2//irの二番目に値が大きいもの + + + + + + + + + + + + + +*****/ + + + + + + + + +/*** + +irは＋-30度まで読めるが,距離が遠い場所ほど減衰 + + + + *3 + *2 *4 + + *1 *5 + + + *0 *6 + + + *11 *7 + + *10 *8 + *9 + +**/ \ No newline at end of file