Akito
GifuRobo B term
Dependencies: FastPWM cal_PID mbed omuni
Fork of
omuni_speed_pid
by 
shinji sawai
Diff: main.cpp
- Revision:
- 1:baab5b88a142
- Parent:
- 0:6da7d0e457a2
- Child:
- 2:c9de02d6d154
--- a/main.cpp Thu Aug 17 03:49:34 2017 +0000
+++ b/main.cpp Wed Aug 23 07:50:14 2017 +0000
@@ -1,96 +1,405 @@
#include "mbed.h"
#include "omuni.hpp"
+#include "cal_PID.hpp"
-int addr[] = {0x10, 0x12, 0x14};
-int armAddr[] = {0x16, 0x18};
+const float omega_max = 1.0 * 2 * 3.14159265f;
+const float wrap_radius = 1.5f;
+const float wrap_speed = 2.0f;
+const int8_t armDuty[] = {-50, -50};
+const signed int spearDuty = 127;
+const int32_t lift_preset[4] = {620, 990, 1360, 1730}; // 12bit 0 ~ 4095
+const int32_t lift_max = 1830;
+const int32_t lift_min = 500;
+
+const int omuniAddr[] = {0x10, 0x12, 0x14}; // 0000 , 1000 , 0100
+const int armAddr[] = {0x16, 0x18}; // 1100 , 0010
+const int spearAddr = 0x1a; // 1010
+const int liftAddr = 0x1c; // 0110
+
+int drive_motor(int address,signed int duty);
+void emergencyStop();
+
+bool ems = false;
+
bool arm = false;
+bool spear = false;
+int lift_targ = lift_preset[0];
+bool first_receive = true;
+int past_lift_gray = 0b00;
+int lift_inc = 0;
+
+//DigitalIn button(USER_BUTTON);
+DigitalOut led(LED1);
Serial p(USBTX, USBRX);
Serial pc(PA_11, PA_12);
I2C i2cMaster(D14, D15);
-// archan
-omuni omuni(&i2cMaster, TIM1, TIM2, TIM3, 800, 2.0f, addr, 0.25f, 0.1f);
+omuni omuni(&i2cMaster, TIM1, TIM2, TIM3, 533, 2.0f, omuniAddr, 0.4704f, 0.1f);
+AnalogIn lift_meter(PC_0);
+DigitalIn spear_sensor(PC_3);
+Timer mesPeriod;
+cal_pid lift_pid;
+
+float speed_x, speed_y, omega;
+bool f;
-float vx, vy, ome;
-bool f, pre_f;
-char recv[3] = {0};
+typedef union{
+ struct{
+ unsigned angle :4;
+ unsigned speed :2;
+ unsigned byteNum0:2;
+ };
+ struct{
+ unsigned wrap_R :1;
+ unsigned wrap_L :1;
+ unsigned coorSys :1;
+ unsigned omega :2;
+ unsigned dir :1;
+ unsigned byteNum1 :2;
+ };
+ struct{
+ unsigned lift_down :1;
+ unsigned lift_up :1;
+ unsigned lift_gray :2;
+ unsigned spear :1;
+ unsigned arm :1;
+ unsigned byteNum2 :2;
+ };
+ struct{
+ unsigned reserved2 :1;
+ unsigned emo :1;
+ unsigned reserved3 :4;
+ unsigned byteNum3 :2;
+ };
+ struct{
+ unsigned control :6;
+ unsigned byteNum :2;
+ };
+ struct{
+ unsigned bit0:1;
+ unsigned bit1:1;
+ unsigned bit2:1;
+ unsigned bit3:1;
+ unsigned bit4:1;
+ unsigned bit5:1;
+ unsigned bit6:1;
+ unsigned bit7:1;
+ };
+ int8_t data;
+}comData_t;
+
+
+comData_t RCData[4] = {0};
+
+int grayDiffer2bit(int gray, int origin){
+
+ switch(origin){
+ case 0b00:
+ switch(gray){
+ case 0b01: return 1;
+ case 0b10: return -1;
+ default : return 0;
+ }
+ case 0b01:
+ switch(gray){
+ case 0b11: return 1;
+ case 0b00: return -1;
+ default : return 0;
+ }
+ case 0b11:
+ switch(gray){
+ case 0b10: return 1;
+ case 0b01: return -1;
+ default : return 0;
+ }
+ case 0b10:
+ switch(gray){
+ case 0b00: return 1;
+ case 0b11: return -1;
+ default : return 0;
+ }
+ }
+ return 0;
+}
+
void pc_rx()
{
- char rtemp = pc.getc();
+ char temp = pc.getc();
+ comData_t rcTemp;
+ rcTemp.data = temp;
+ float rad, speed;
+ int lift_nearest_num = 0;
- if((rtemp & 0b11000000) == 0b00000000) recv[0] = rtemp;
- else if((rtemp & 0b11000000) == 0b01000000) recv[1] = rtemp;
- else if((rtemp & 0b11000000) == 0b10000000) recv[2] = rtemp;
+
+ RCData[rcTemp.byteNum] = rcTemp;
- float direc = (recv[0] & 0x0f) * 3.141592 / 8.0;
- float speed = 0.8 * ((recv[0] & 0b00110000) >> 4);
- vx = speed * cos(direc) * -1;
- vy = speed * sin(direc) * -1;
+ /*
+ p.printf("%d",rcTemp.bit7);
+ p.printf("%d",rcTemp.bit6);
+ p.printf("%d",rcTemp.bit5);
+ p.printf("%d",rcTemp.bit4);
+ p.printf("%d",rcTemp.bit3);
+ p.printf("%d",rcTemp.bit2);
+ p.printf("%d",rcTemp.bit1);
+ p.printf("%d",rcTemp.bit0);
+ p.printf("\n");
+ */
+ //p.printf("%d\n", rcTemp.byteNum);
+ //p.printf("speed = %d, angle = %d\n",rcTemp.speed, rcTemp.angle);
- if(recv[2] & 0b1)
- {
- arm = true;
- }
- else
- {
- arm = false;
+ switch(temp >> 6){
+ case 0:
+ //p.printf("speed = %d, angle = %d\n",rcTemp.speed, rcTemp.angle);
+ break;
+ case 1:
+ //p.printf("dir = %d, omega = %d, coorSys = %d\n", rcTemp.dir, rcTemp.omega, rcTemp.coorSys);
+ break;
+ case 2:
+ //p.printf("arm = %d, spear = %d, lift_gray = %d, lift_up = %d, lift_down = %d\n",rcTemp.arm, rcTemp.spear, rcTemp.lift_gray, rcTemp.lift_up, rcTemp.lift_down);
+ break;
+ case 3:
+
+ break;
}
- if(recv[1] & 0b11000)
- {
- f = false;
- ome = ((recv[1] & 0b11000) >> 3) * 3.141592 / 2.0;
- if(recv[1] & 0b100000) ome *= -1;
+ if(rcTemp.byteNum == 3){ // データは揃った
+
+ led = !led;
+
+ ems = RCData[3].emo != 0;
+
+ rad = RCData[0].angle * 3.141592 / 8.0;
+ speed = 0.8 * RCData[0].speed;
+ speed_x = speed * cos(rad) * -1;
+ speed_y = speed * sin(rad) * -1;
+
+ f = RCData[1].coorSys;
+ omega = RCData[1].omega * omega_max / 3.0f;
+ omega *= RCData[1].dir ? 1 : -1;
+
+
+ if(RCData[1].wrap_R || RCData[1].wrap_L){
+ speed_x = wrap_speed;
+ speed_y = 0;
+ f = 0;
+ omega = wrap_speed / wrap_radius;
+ speed_x *= RCData[1].wrap_L ? 1 : -1;
+ omega *= RCData[1].wrap_L ? 1 : -1;
+ }
+
+
+ arm = RCData[2].arm != 0;
+
+ spear = RCData[2].spear != 0;
+
+ if(first_receive){
+ past_lift_gray = RCData[2].lift_gray;
+ first_receive = false;
+ }
+ if(RCData[2].lift_gray != past_lift_gray){ // 段階移動
+ int lift_diff = 0;
+ int lift_nearest = 10000;
+ lift_diff = grayDiffer2bit(RCData[2].lift_gray, past_lift_gray);
+ //p.printf("%d --> %d\n", past_lift_gray, RCData[2].lift_gray);
+ //p.printf("gray = %d\n", lift_diff);
+ for(int num = 0; num < 4; num++){ // 最寄りの段階
+ //p.printf("nearest = %d, num = %d\n", lift_nearest, lift_nearest_num);
+ if(lift_nearest > abs(lift_preset[num] - lift_targ)){
+ lift_nearest_num = num;
+ lift_nearest = abs(lift_preset[num] - lift_targ);
+ }
+ }
+ //p.printf("lift_targ = %d num = %d\n", lift_targ, lift_nearest_num);
+ if(lift_diff == 1){
+ if(lift_preset[lift_nearest_num] >= lift_targ){
+ if(lift_nearest_num < 3)lift_targ = lift_preset[lift_nearest_num + 1];
+ }
+ else{
+ lift_targ = lift_preset[lift_nearest_num];
+ }
+ }
+ else if(lift_diff == -1){
+ if(lift_preset[lift_nearest_num] <= lift_targ){
+ if(lift_nearest_num > 0)lift_targ = lift_preset[lift_nearest_num - 1];
+ }
+ else{
+ lift_targ = lift_preset[lift_nearest_num];
+ }
+ }
+ }
+ past_lift_gray = RCData[2].lift_gray;
+
+ if(RCData[2].lift_down != 0)lift_inc = -5;
+ else if(RCData[2].lift_up != 0)lift_inc = 5;
+ else lift_inc = 0;
+
+ //p.printf("inc = %d\n",lift_inc);
+ //p.printf("lift_targ = %d\n", lift_targ);
+
+ } // すっきりした // してない
+
+}
+
+
+void arm_control(){
+ char armData[2] = {0};
+
+ armData[0] = arm? armDuty[0] : 0 ;
+ armData[1] = arm? armDuty[1] : 0 ;
+
+ i2cMaster.write(armAddr[0], &armData[0], 1);
+ i2cMaster.write(armAddr[1], &armData[1], 1);
+
+}
+
+void lift_control(){
+ int32_t meter = 0;
+ signed int duty = 0;
+
+ if(lift_inc < 0){
+ if(lift_min < lift_targ + lift_inc)lift_targ += lift_inc;
}
- else
- {
- if(recv[1] & 0b11)
- {
- f = true;
-// if(pre_f == false) omuni.reset_theta();
- }
- else
- {
- f = false;
- }
- ome = (recv[1] & 0b11) * 3.141592 / 2.0;
- if((recv[1] & 0b100)) ome *= -1;
+ else if(lift_inc > 0){
+ if(lift_max > lift_targ + lift_inc)lift_targ += lift_inc;
}
- pre_f = f;
+ meter = lift_meter.read_u16() >> 4;
+ duty = lift_pid.get_pid(meter, lift_targ);
+ drive_motor(liftAddr, duty);
+
+}
+
+void spear_control(){
+ static int spear_state = 0;
+ signed int duty = 0;
+ int sensor = spear_sensor; // リミット時に1
+
+ switch(spear_state){
+ case 0 : // 待ち
+ if(spear){
+ spear_state = 1;
+ }
+ else{
+ if(sensor == 0){
+ duty = spearDuty * -1;
+ }
+ }
+ break;
+ case 1 : // 初期位置から抜け出す
+ duty = spearDuty;
+ if(sensor == 0){
+ spear_state = 2;
+ }
+ break;
+ case 2 : // 到達位置まで動かす
+ if(sensor == 0){
+ duty = spearDuty;
+ }
+ else{
+ spear_state = 3;
+ }
+ break;
+ case 3 : // 到達位置から抜け出す
+ duty = spearDuty * -1;
+ if(sensor == 0){
+ spear_state = 4;
+ }
+ break;
+ case 4 : // 初期位置まで戻る
+ if(sensor == 0){
+ duty = spearDuty * -1;
+ }
+ else{
+ spear_state = 0;
+ }
+ break;
+ default :
+ break;
+ }
+ drive_motor(spearAddr, duty);
+}
+
+
+void control(){
+
+ arm_control();
+ lift_control();
+ spear_control();
+
}
int main()
{
+ int period_us = 0;
+ int count = 0;
+
p.baud(115200);
pc.baud(9600);
- pc.printf("Hello!\n");
+ p.printf("Hello!\n");
pc.attach(pc_rx, Serial::RxIrq);
+
+ //i2cMaster.frequency(400000);
+
omuni.set_speed(0.0f, 0.0f);
- // archan
omuni.set_pid(0, 3.0f, 0.07f, 0.05f);
omuni.set_pid(1, 3.0f, 0.07f, 0.05f);
omuni.set_pid(2, 3.0f, 0.07f, 0.05f);
+ lift_pid.param(0.3, 0.0, 0.0);
+ lift_pid.period(0.001);
+ lift_pid.output(-127, 127);
+
+ //control_loop.attach_us(&control, 1000);
+
+ led = 0;
+
+ //mesPeriod.start();
+
while(1)
{
- wait(0.001);
- omuni.set_speed(vx, vy, ome, f);
+ wait(0.0001);
+ omuni.set_speed(speed_x, speed_y, omega, f);
omuni.drive();
+ control();
- if(arm)
- {
- char send = -127;
- i2cMaster.write(armAddr[0], &send, 1);
- i2cMaster.write(armAddr[1], &send, 1);
+ if(ems){
+ emergencyStop();
+ }
+ /*
+ if(count < 100){
+ count += 1;
+ }
+ else{
+ period_us = mesPeriod.read_us();
+ mesPeriod.reset();
+
+ p.printf("%dHz\n", 1000000 * 100 / period_us);
+
+ count = 0;
}
- else
- {
- char send = 0;
- i2cMaster.write(armAddr[0], &send, 1);
- i2cMaster.write(armAddr[1], &send, 1);
- }
-
- p.printf("%f\n", omuni.get_theta());
+ */
}
-}
\ No newline at end of file
+}
+
+int drive_motor(int address,signed int duty){ /* アドレスを指定してモータを駆動 */
+ char send_data;
+ int ack;
+ if((duty>127)||(duty<-128))return -1; /* 範囲外なら送信しない */
+ send_data=duty;
+ ack=i2cMaster.write(address,&send_data,1);
+ return ack;
+}
+
+
+void emergencyStop(){
+ drive_motor(omuniAddr[0], 0);
+ drive_motor(omuniAddr[1], 0);
+ drive_motor(omuniAddr[2], 0);
+ drive_motor(armAddr[0], 0);
+ drive_motor(armAddr[1], 0);
+ drive_motor(liftAddr, 0);
+ drive_motor(spearAddr, 0);
+ while(1);
+}
+
+