Austin Buchan / mbed_zumy_packet

Publishing for Biomimetics.

Dependencies:   CRC MODDMA MODSERIAL MPU6050IMU PID QEI mbed-rtos mbed-src

main.cpp@0:8cfa73bb68e4, 2016-05-31 (annotated)

Committer:
abuchan
Date:
Tue May 31 17:04:59 2016 +0000
Revision:
0:8cfa73bb68e4
Publishing for Biomimetics.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
abuchan 0:8cfa73bb68e4 1 #include "mbed.h"
abuchan 0:8cfa73bb68e4 2
abuchan 0:8cfa73bb68e4 3 #include "protocol.h"
abuchan 0:8cfa73bb68e4 4 #include "packet_parser.h"
abuchan 0:8cfa73bb68e4 5 #include "sensors.h"
abuchan 0:8cfa73bb68e4 6 #include "control.h"
abuchan 0:8cfa73bb68e4 7
abuchan 0:8cfa73bb68e4 8 #define PID_KP 1.0f
abuchan 0:8cfa73bb68e4 9 #define PID_KI 0.1f
abuchan 0:8cfa73bb68e4 10 #define PID_KD 0.0f
abuchan 0:8cfa73bb68e4 11
abuchan 0:8cfa73bb68e4 12 #define PID_PERIOD 0.01f
abuchan 0:8cfa73bb68e4 13
abuchan 0:8cfa73bb68e4 14 #define PID_IN_MIN (-50.0f)
abuchan 0:8cfa73bb68e4 15 #define PID_IN_MAX 50.0f
abuchan 0:8cfa73bb68e4 16 #define PID_OUT_MIN (-1.0f)
abuchan 0:8cfa73bb68e4 17 #define PID_OUT_MAX 1.0f
abuchan 0:8cfa73bb68e4 18
abuchan 0:8cfa73bb68e4 19 #define TICK_PER_REV 1200
abuchan 0:8cfa73bb68e4 20
abuchan 0:8cfa73bb68e4 21 #define PID_DEAD_BAND 0.03f
abuchan 0:8cfa73bb68e4 22
abuchan 0:8cfa73bb68e4 23 #define SERIAL_BAUDRATE 230400
abuchan 0:8cfa73bb68e4 24
abuchan 0:8cfa73bb68e4 25 #define VOLTAGE_PIN p15
abuchan 0:8cfa73bb68e4 26
abuchan 0:8cfa73bb68e4 27 #define L_ENC_A_PIN p29
abuchan 0:8cfa73bb68e4 28 #define L_ENC_B_PIN p30
abuchan 0:8cfa73bb68e4 29 #define R_ENC_A_PIN p11
abuchan 0:8cfa73bb68e4 30 #define R_ENC_B_PIN p12
abuchan 0:8cfa73bb68e4 31
abuchan 0:8cfa73bb68e4 32 #define IMU_SDA_PIN p9
abuchan 0:8cfa73bb68e4 33 #define IMU_SCL_PIN p10
abuchan 0:8cfa73bb68e4 34
abuchan 0:8cfa73bb68e4 35 #define L_MOT_0_PIN p21
abuchan 0:8cfa73bb68e4 36 #define L_MOT_1_PIN p22
abuchan 0:8cfa73bb68e4 37 #define R_MOT_0_PIN p23
abuchan 0:8cfa73bb68e4 38 #define R_MOT_1_PIN p24
abuchan 0:8cfa73bb68e4 39
abuchan 0:8cfa73bb68e4 40 void fill_time_packet(packet_t* pkt, uint32_t time) {
abuchan 0:8cfa73bb68e4 41 pkt->header.type = PKT_TYPE_TIME;
abuchan 0:8cfa73bb68e4 42 pkt->header.length = sizeof(header_t) + sizeof(time_data_t) + 1;
abuchan 0:8cfa73bb68e4 43 time_data_t* time_data = (time_data_t*)pkt->data_crc;
abuchan 0:8cfa73bb68e4 44 time_data->time = time;
abuchan 0:8cfa73bb68e4 45 }
abuchan 0:8cfa73bb68e4 46
abuchan 0:8cfa73bb68e4 47 extern "C" void mbed_reset();
abuchan 0:8cfa73bb68e4 48
abuchan 0:8cfa73bb68e4 49 int main() {
abuchan 0:8cfa73bb68e4 50
abuchan 0:8cfa73bb68e4 51 DigitalOut led1(LED1);
abuchan 0:8cfa73bb68e4 52 DigitalOut led4(LED4);
abuchan 0:8cfa73bb68e4 53
abuchan 0:8cfa73bb68e4 54 led1 = 1;
abuchan 0:8cfa73bb68e4 55
abuchan 0:8cfa73bb68e4 56 Timer system_timer;
abuchan 0:8cfa73bb68e4 57
abuchan 0:8cfa73bb68e4 58 system_timer.start();
abuchan 0:8cfa73bb68e4 59 uint32_t last_time = system_timer.read_ms();
abuchan 0:8cfa73bb68e4 60 uint32_t current_time = last_time;
abuchan 0:8cfa73bb68e4 61
abuchan 0:8cfa73bb68e4 62 PacketParser parser(SERIAL_BAUDRATE, USBTX, USBRX, LED2, LED3);
abuchan 0:8cfa73bb68e4 63
abuchan 0:8cfa73bb68e4 64 packet_union_t* recv_pkt = NULL;
abuchan 0:8cfa73bb68e4 65 packet_union_t* send_pkt = NULL;
abuchan 0:8cfa73bb68e4 66 command_data_t* command;
abuchan 0:8cfa73bb68e4 67
abuchan 0:8cfa73bb68e4 68 send_pkt = parser.get_send_packet();
abuchan 0:8cfa73bb68e4 69 if (send_pkt != NULL) {
abuchan 0:8cfa73bb68e4 70 fill_time_packet(&(send_pkt->packet), system_timer.read_us());
abuchan 0:8cfa73bb68e4 71 parser.send_packet(send_pkt);
abuchan 0:8cfa73bb68e4 72 }
abuchan 0:8cfa73bb68e4 73
abuchan 0:8cfa73bb68e4 74 Sensors sensors(
abuchan 0:8cfa73bb68e4 75 &system_timer,
abuchan 0:8cfa73bb68e4 76 VOLTAGE_PIN,
abuchan 0:8cfa73bb68e4 77 L_ENC_A_PIN, L_ENC_B_PIN,
abuchan 0:8cfa73bb68e4 78 R_ENC_A_PIN, R_ENC_B_PIN, TICK_PER_REV,
abuchan 0:8cfa73bb68e4 79 IMU_SDA_PIN, IMU_SCL_PIN
abuchan 0:8cfa73bb68e4 80 );
abuchan 0:8cfa73bb68e4 81
abuchan 0:8cfa73bb68e4 82 Control control(
abuchan 0:8cfa73bb68e4 83 L_MOT_0_PIN, L_MOT_1_PIN, R_MOT_0_PIN, R_MOT_1_PIN,
abuchan 0:8cfa73bb68e4 84 &sensors, TICK_PER_REV,
abuchan 0:8cfa73bb68e4 85 PID_KP, PID_KI, PID_KD, PID_PERIOD, PID_IN_MAX, PID_DEAD_BAND
abuchan 0:8cfa73bb68e4 86 );
abuchan 0:8cfa73bb68e4 87
abuchan 0:8cfa73bb68e4 88 led4 = 1;
abuchan 0:8cfa73bb68e4 89
abuchan 0:8cfa73bb68e4 90 packet_union_t* sensor_pkt = parser.get_send_packet();
abuchan 0:8cfa73bb68e4 91
abuchan 0:8cfa73bb68e4 92 while(1) {
abuchan 0:8cfa73bb68e4 93
abuchan 0:8cfa73bb68e4 94 recv_pkt = parser.get_received_packet();
abuchan 0:8cfa73bb68e4 95
abuchan 0:8cfa73bb68e4 96 if (recv_pkt != NULL) {
abuchan 0:8cfa73bb68e4 97
abuchan 0:8cfa73bb68e4 98 switch (recv_pkt->packet.header.type) {
abuchan 0:8cfa73bb68e4 99
abuchan 0:8cfa73bb68e4 100 case PKT_TYPE_RESET:
abuchan 0:8cfa73bb68e4 101 mbed_reset();
abuchan 0:8cfa73bb68e4 102 break;
abuchan 0:8cfa73bb68e4 103
abuchan 0:8cfa73bb68e4 104 case PKT_TYPE_COMMAND:
abuchan 0:8cfa73bb68e4 105 command = (command_data_t*)recv_pkt->packet.data_crc;
abuchan 0:8cfa73bb68e4 106 control.set_setpoints(command->left, command->right);
abuchan 0:8cfa73bb68e4 107 break;
abuchan 0:8cfa73bb68e4 108
abuchan 0:8cfa73bb68e4 109 case PKT_TYPE_TIME:
abuchan 0:8cfa73bb68e4 110 send_pkt = parser.get_send_packet();
abuchan 0:8cfa73bb68e4 111 if (send_pkt != NULL) {
abuchan 0:8cfa73bb68e4 112 fill_time_packet(&(send_pkt->packet), system_timer.read_us());
abuchan 0:8cfa73bb68e4 113 parser.send_packet(send_pkt);
abuchan 0:8cfa73bb68e4 114 }
abuchan 0:8cfa73bb68e4 115 break;
abuchan 0:8cfa73bb68e4 116
abuchan 0:8cfa73bb68e4 117 case PKT_TYPE_READ:
abuchan 0:8cfa73bb68e4 118 if (sensor_pkt != NULL) {
abuchan 0:8cfa73bb68e4 119 if(sensors.fill_sensor_packet(&(sensor_pkt->packet))) {
abuchan 0:8cfa73bb68e4 120 control.fill_sensor_packet(&(sensor_pkt->packet));
abuchan 0:8cfa73bb68e4 121 parser.send_packet(sensor_pkt);
abuchan 0:8cfa73bb68e4 122 sensor_pkt = parser.get_send_packet();
abuchan 0:8cfa73bb68e4 123 }
abuchan 0:8cfa73bb68e4 124 } else {
abuchan 0:8cfa73bb68e4 125 sensor_pkt = parser.get_send_packet();
abuchan 0:8cfa73bb68e4 126 }
abuchan 0:8cfa73bb68e4 127 break;
abuchan 0:8cfa73bb68e4 128 }
abuchan 0:8cfa73bb68e4 129
abuchan 0:8cfa73bb68e4 130 parser.free_received_packet(recv_pkt);
abuchan 0:8cfa73bb68e4 131 }
abuchan 0:8cfa73bb68e4 132
abuchan 0:8cfa73bb68e4 133 current_time = system_timer.read_ms();
abuchan 0:8cfa73bb68e4 134
abuchan 0:8cfa73bb68e4 135 if (current_time - last_time > 500) {
abuchan 0:8cfa73bb68e4 136 last_time = current_time;
abuchan 0:8cfa73bb68e4 137 led1 = !led1;
abuchan 0:8cfa73bb68e4 138 led4 = !led4;
abuchan 0:8cfa73bb68e4 139 }
abuchan 0:8cfa73bb68e4 140
abuchan 0:8cfa73bb68e4 141 Thread::yield();
abuchan 0:8cfa73bb68e4 142 }
abuchan 0:8cfa73bb68e4 143 }