直進しかさせてくれない補助輪のグリップが駆動輪と同じだと
上手く曲がれんので駆動輪にゴム系接着剤、スーパーXを薄く塗って
ゴムタイヤ風にしてみた。
ミクちゃんが持ってるのはハンドルじゃなくてコネクタ・・・
ボディつけたらESP-01が入らん。。。 どうしたもんか...
いい感じのグリップになったので超信地旋回もできるように
ソフトをちょっと変えた。。。
ちょっと・・・って言ってもちょっと面倒だった。
DRV8830を使うと単純なスイッチ切り替えのL9110とかと違って
コイル間の電圧を直接制御できるので（実際にはPWMで分圧だけど）パルスとか
意識せずにベクトル制御とかマイクロステップ駆動もそんなに難しくなくできる。
けど、動かしてみるとハーフステップ駆動でもXYベクトルの大きさ合わせてやると
そこそこ滑らかに回ったのでマイクロステップとかはやってない。
その代わりに制御がI2Cなもんで割込みの中で動かすにはちょっと具合悪い。
ただ車輪のダイレクトドライブとかならプロペラをぶんぶん回すみたいな
高回転じゃないのでポーリングでも制御が十分できる。
低回転でゆっくり電圧変えるならそんなに困らない。。。けど、
最初の作り込みが甘かった。
2個のモータを同じ方向に回転させるのは簡単にできるようにしちゃったもんだから
片方が逆回転だとややこしくなってしまった。
他人が見ても？？？だと思うけど、おぼえ・・・
これまでのいろいろロボのつぎはぎでとりあえずのテスト・・・に
付け足し、付け足しなんでいけてない。。。
// _1
//
// _2 モータ2個独立して動かせるようにする
// M5stickからの受信データは一桁配列番号：0～9、 送信データは2桁番号 10～
// joy_x：[1], joy_y[2] count[11], Batt[12]
//
// _3 joy_x:1 joy_y:2 で独立駆動テスト 値が -100 ~ 100 ＋のみ使って一方向回転でまずテスト
// 位相時間の最小値は 103－joy値 ：3ms としてみる。
//
// _4 逆回転もマップ反転処理を入れて対応
// 一応、2個モータ独立で反対回転もjoyコントローラでできるようになった。
//
// _5 回転の変化がリニアじゃないのでリニア風にしてみる
// _5a コメントゴミのある程度整理
//
// _6 モータをミクちゃんに左右逆に付けてしまったので ベースマップのR側を反転マップにした
//
// _7 モータドライバ基板小さく切ったやつは具合悪くてオリジナル基板でミクちゃんに組付けたら前後、左右が逆になってしまった
// _5aの回転方向で、左右を入れ替え
//
//
// _8 超信地旋回検討
// joyy 10以下 で joyxが50以上になれば信地旋回始める
//
//
#include <ESP8266WiFi.h>
#include <espnow.h>
#define CHANNEL 0
#define LED 2
#include <Wire.h>
// 共立DRV8830、2個モジュール デフォルト設定アドレス
#define MOTOR_L 0x63 //A0=high, A1=open 8bitベースアドレス:0xC6 --- 7-1bit値変換で0x63 IC1側
#define MOTOR_R 0x65 //A0=low, A1=open 8bitベースアドレス:0xCA --- 7-1bit値変換で0x65 IC2側
//共立DRV8830、2個モジュール ★JP1ショート★ A1:GND設定時
#define MOTOR_L2 0x60 //A0=high, A1=open
#define MOTOR_R2 0x62 //A0=low, A1=open
uint8_t slaveAddress[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; // ブロードキャスト
unsigned char recvData[100];
static int8_t sendData[100];
ADC_MODE(ADC_VCC);
unsigned long ms10,ms100,ms200,ms500,ms1000,ms2000;
int8_t Batt,Batt_old,count;
int16_t L[10],R[10],L_base[10],R_base[10];
int dir = 1,motor_sw,plus_delay;
uint8_t M_power,dir1,dir2;
float k_plus=1.0,step_gain =1.0;
int8_t temp;
unsigned long plus_t_old,plus_t_old2,shin_delay_old;
int ph_No,ph_No2,p_delay=100,p_delay2=100,shin_delay=100;
int joy_x,joy_y,joy_y_old;
float v_power1,v_power2,balance;
void setup() {
setupESPNOW();
Wire.pins(0, 2); // GPIO 0 and 2 on ESP-01
Wire.begin(0, 2); // 0=sda, 2=scl
Wire.setClock(400000);
//ハーフステップマップ
L_base[0]= 100; R_base[0]= 0;
L_base[1]= 71; R_base[1]= 71;
L_base[2]= 0; R_base[2]= 100;
L_base[3]= -71; R_base[3]= 71;
L_base[4]=-100; R_base[4]= 0;
L_base[5]= -71; R_base[5]= -71;
L_base[6]= 0; R_base[6]=-100;
L_base[7]= 71; R_base[7]= -71;
for(int i=0; i<8; i++){
L[i]=L_base[i]; R[i]= R_base[i];
}
plus_delay = (int)(p_delay*k_plus);
delay(2000);
ms10=ms100=ms200=ms500=ms1000=ms2000=millis();
}
void loop() {
if (joy_y != 0){
if(ph_No > 7) ph_No = 0; // 0 - 7
if(ph_No2 > 7) ph_No2 = 0; // 0 - 7
//////////////////////////////////////////////////////////////////////
if(abs(joy_y) > 10){
if( millis() > (plus_t_old + p_delay ) ){
drvStep(MOTOR_L2 ,(int16_t)(L[ ph_No ]*step_gain));
drvStep(MOTOR_R2 ,(int16_t)(R[ ph_No ]*step_gain));
plus_t_old = millis();
ph_No++;
}
if( millis() > (plus_t_old2 + p_delay2 ) ){
drvStep(MOTOR_L,(int16_t)(L[ ph_No2 ]*step_gain));
drvStep(MOTOR_R,(int16_t)(R[ ph_No2 ]*step_gain));
plus_t_old2 = millis();
ph_No2++;
}
}
else{
if( abs(joy_x) > 50 ){ // 超信地旋回
shin_delay = (103 - abs(joy_x) );
if( millis() > (shin_delay_old + shin_delay ) ){
if(joy_x > 0) for(int i=0; i<8; i++) R[i]= -R_base[i];// 正回転マップ
else for(int i=0; i<8; i++) R[i]= R_base[i];// 逆回転マップ
drvStep(MOTOR_L2 ,(int16_t)(L[ ph_No ]*step_gain));
drvStep(MOTOR_R2 ,(int16_t)(R[ ph_No ]*step_gain));
if(joy_x > 0) for(int i=0; i<8; i++) R[i]= R_base[i];// 逆回転ップ
else for(int i=0; i<8; i++) R[i]= -R_base[i];// 正回転マップ
drvStep(MOTOR_L, (int16_t)(L[ ph_No ]*step_gain));
drvStep(MOTOR_R, (int16_t)(R[ ph_No ]*step_gain));
shin_delay_old = millis();
ph_No++;
}
}
}
//////////////////////////////////////////////////////////////////////
}
else{
drvStep(MOTOR_L , 0); drvStep(MOTOR_R , 0);
drvStep(MOTOR_L2, 0); drvStep(MOTOR_R2, 0);
}
if (millis() > ms100){
joy_x = recvData[1]-100;
joy_y = recvData[2]-100;
////// パルス位相変化時間を直線単純に反比例・・・これだと最後に急に速くなる感じになる。
// p_delay = (103 - abs(joy_y) ) ;
// p_delay2 = (103 - abs(joy_y) ) ;
////// 非線形にしてみた ///////////////////////////////////////////////
if( joy_y != 0 ){
p_delay = (int)( 1.0/(float)abs(joy_y) *300.0 );
p_delay2 = (int)( 1.0/(float)abs(joy_y) *300.0 );
}
if( joy_x > 20){
p_delay = p_delay + joy_x * joy_x * 0.002 ;
}
if( joy_x < 20){
p_delay2 = p_delay2 + joy_x * joy_x * 0.002 ;
}
if( (joy_y_old >= 0) && (joy_y < 0) ){
for(int i=0; i<8; i++) R[i]= -R_base[i];// 正回転マップ
}
if( (joy_y_old <= 0) && (joy_y > 0) ){
for(int i=0; i<8; i++) R[i]= R_base[i];// 逆回転マップ
}
joy_y_old = joy_y;
ms100+=100;
}
if (millis() > ms1000){ //ESPNOW 返信/////////////////////////
Batt = ESP.getVcc()*0.01;// 受信側でｘ0.1で V
// if( Batt_old != Batt){
sendData[12] = Batt; // 0.1V分を桁上げ
esp_now_send(slaveAddress, (uint8_t*)sendData, 100);
// }
// Batt_old = Batt;
count++;
sendData[11] = count;
ms1000+=1000;
}
}
void setupESPNOW(){
WiFi.mode(WIFI_STA);
WiFi.disconnect();
esp_now_init();
esp_now_set_self_role(ESP_NOW_ROLE_COMBO);
esp_now_register_recv_cb(onRecv);
// esp_now_peer_info_t peerInfo={};
// memcpy(peerInfo.peer_addr, slaveAddress, 6);
// peerInfo.channel = CHANNEL;
// peerInfo.encrypt = false;
// esp_now_add_peer(&peerInfo);
}
void onRecv(uint8_t* mac_addr, uint8_t* on_RecvData, uint8_t len) {
for (int i=0; i<len; i++) recvData[i] = on_RecvData[i];
}
////// DRV8830 ///////////////////////////////////////////////////////////////////
void drvStep(byte adr, int16_t pwr) { //pwr -100 to 100
byte dir, st;
pwr = constrain (pwr, -100, 100);
if (pwr < 0) dir = 2;
else dir =1;
byte ipower=(byte) (abs(63*pwr*0.01));
if (ipower == 0) dir=0; //惰性
//if (ipower == 0) dir=3; //ブレーキ
st = drv8830read(adr);
if (st & 1) drv8830write(adr, 0, 0);
drv8830write(adr, ipower, dir);
}
void drv8830write(byte adr, byte pwm, byte ctrl) { // pwm 6~3F、ctrl 0:フリー 1:正転 2:逆転 3;ブレーキ
Wire.beginTransmission(adr);
Wire.write(0);
Wire.write(pwm*4+ctrl);
Wire.endTransmission(true);
}
int drv8830read(byte adr) {
Wire.beginTransmission(adr);
Wire.write(1);
Wire.endTransmission(false);
Wire.requestFrom((int)adr, (int)1, (int)1);
return Wire.read();
}
