shinji sawai / Mbed 2 deprecated omuni_speed_pid

omuni

Dependencies:   mbed nucleo_rotary_encoder

main.cpp@0:6da7d0e457a2, 2017-08-17 (annotated)

Committer:
sawai
Date:
Thu Aug 17 03:49:34 2017 +0000
Revision:
0:6da7d0e457a2
omuni

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sawai 0:6da7d0e457a2 1 #include "mbed.h"
sawai 0:6da7d0e457a2 2 #include "omuni.hpp"
sawai 0:6da7d0e457a2 3
sawai 0:6da7d0e457a2 4 int addr[] = {0x10, 0x12, 0x14};
sawai 0:6da7d0e457a2 5 int armAddr[] = {0x16, 0x18};
sawai 0:6da7d0e457a2 6 bool arm = false;
sawai 0:6da7d0e457a2 7
sawai 0:6da7d0e457a2 8 Serial p(USBTX, USBRX);
sawai 0:6da7d0e457a2 9 Serial pc(PA_11, PA_12);
sawai 0:6da7d0e457a2 10 I2C i2cMaster(D14, D15);
sawai 0:6da7d0e457a2 11 // archan
sawai 0:6da7d0e457a2 12 omuni omuni(&i2cMaster, TIM1, TIM2, TIM3, 800, 2.0f, addr, 0.25f, 0.1f);
sawai 0:6da7d0e457a2 13
sawai 0:6da7d0e457a2 14 float vx, vy, ome;
sawai 0:6da7d0e457a2 15 bool f, pre_f;
sawai 0:6da7d0e457a2 16 char recv[3] = {0};
sawai 0:6da7d0e457a2 17
sawai 0:6da7d0e457a2 18 void pc_rx()
sawai 0:6da7d0e457a2 19 {
sawai 0:6da7d0e457a2 20 char rtemp = pc.getc();
sawai 0:6da7d0e457a2 21
sawai 0:6da7d0e457a2 22 if((rtemp & 0b11000000) == 0b00000000) recv[0] = rtemp;
sawai 0:6da7d0e457a2 23 else if((rtemp & 0b11000000) == 0b01000000) recv[1] = rtemp;
sawai 0:6da7d0e457a2 24 else if((rtemp & 0b11000000) == 0b10000000) recv[2] = rtemp;
sawai 0:6da7d0e457a2 25
sawai 0:6da7d0e457a2 26 float direc = (recv[0] & 0x0f) * 3.141592 / 8.0;
sawai 0:6da7d0e457a2 27 float speed = 0.8 * ((recv[0] & 0b00110000) >> 4);
sawai 0:6da7d0e457a2 28 vx = speed * cos(direc) * -1;
sawai 0:6da7d0e457a2 29 vy = speed * sin(direc) * -1;
sawai 0:6da7d0e457a2 30
sawai 0:6da7d0e457a2 31 if(recv[2] & 0b1)
sawai 0:6da7d0e457a2 32 {
sawai 0:6da7d0e457a2 33 arm = true;
sawai 0:6da7d0e457a2 34 }
sawai 0:6da7d0e457a2 35 else
sawai 0:6da7d0e457a2 36 {
sawai 0:6da7d0e457a2 37 arm = false;
sawai 0:6da7d0e457a2 38 }
sawai 0:6da7d0e457a2 39
sawai 0:6da7d0e457a2 40 if(recv[1] & 0b11000)
sawai 0:6da7d0e457a2 41 {
sawai 0:6da7d0e457a2 42 f = false;
sawai 0:6da7d0e457a2 43 ome = ((recv[1] & 0b11000) >> 3) * 3.141592 / 2.0;
sawai 0:6da7d0e457a2 44 if(recv[1] & 0b100000) ome *= -1;
sawai 0:6da7d0e457a2 45 }
sawai 0:6da7d0e457a2 46 else
sawai 0:6da7d0e457a2 47 {
sawai 0:6da7d0e457a2 48 if(recv[1] & 0b11)
sawai 0:6da7d0e457a2 49 {
sawai 0:6da7d0e457a2 50 f = true;
sawai 0:6da7d0e457a2 51 // if(pre_f == false) omuni.reset_theta();
sawai 0:6da7d0e457a2 52 }
sawai 0:6da7d0e457a2 53 else
sawai 0:6da7d0e457a2 54 {
sawai 0:6da7d0e457a2 55 f = false;
sawai 0:6da7d0e457a2 56 }
sawai 0:6da7d0e457a2 57 ome = (recv[1] & 0b11) * 3.141592 / 2.0;
sawai 0:6da7d0e457a2 58 if((recv[1] & 0b100)) ome *= -1;
sawai 0:6da7d0e457a2 59 }
sawai 0:6da7d0e457a2 60 pre_f = f;
sawai 0:6da7d0e457a2 61 }
sawai 0:6da7d0e457a2 62
sawai 0:6da7d0e457a2 63 int main()
sawai 0:6da7d0e457a2 64 {
sawai 0:6da7d0e457a2 65 p.baud(115200);
sawai 0:6da7d0e457a2 66 pc.baud(9600);
sawai 0:6da7d0e457a2 67 pc.printf("Hello!\n");
sawai 0:6da7d0e457a2 68 pc.attach(pc_rx, Serial::RxIrq);
sawai 0:6da7d0e457a2 69 omuni.set_speed(0.0f, 0.0f);
sawai 0:6da7d0e457a2 70 // archan
sawai 0:6da7d0e457a2 71 omuni.set_pid(0, 3.0f, 0.07f, 0.05f);
sawai 0:6da7d0e457a2 72 omuni.set_pid(1, 3.0f, 0.07f, 0.05f);
sawai 0:6da7d0e457a2 73 omuni.set_pid(2, 3.0f, 0.07f, 0.05f);
sawai 0:6da7d0e457a2 74
sawai 0:6da7d0e457a2 75 while(1)
sawai 0:6da7d0e457a2 76 {
sawai 0:6da7d0e457a2 77 wait(0.001);
sawai 0:6da7d0e457a2 78 omuni.set_speed(vx, vy, ome, f);
sawai 0:6da7d0e457a2 79 omuni.drive();
sawai 0:6da7d0e457a2 80
sawai 0:6da7d0e457a2 81 if(arm)
sawai 0:6da7d0e457a2 82 {
sawai 0:6da7d0e457a2 83 char send = -127;
sawai 0:6da7d0e457a2 84 i2cMaster.write(armAddr[0], &send, 1);
sawai 0:6da7d0e457a2 85 i2cMaster.write(armAddr[1], &send, 1);
sawai 0:6da7d0e457a2 86 }
sawai 0:6da7d0e457a2 87 else
sawai 0:6da7d0e457a2 88 {
sawai 0:6da7d0e457a2 89 char send = 0;
sawai 0:6da7d0e457a2 90 i2cMaster.write(armAddr[0], &send, 1);
sawai 0:6da7d0e457a2 91 i2cMaster.write(armAddr[1], &send, 1);
sawai 0:6da7d0e457a2 92 }
sawai 0:6da7d0e457a2 93
sawai 0:6da7d0e457a2 94 p.printf("%f\n", omuni.get_theta());
sawai 0:6da7d0e457a2 95 }
sawai 0:6da7d0e457a2 96 }