Austin Buchan / mbed_BLDC_driver_KL25Z

BLDC control for jumping robot.

Dependencies:   CRC MODSERIAL mbed-dev mbed-rtos

Fork of mbed_BLDC_driver_KL25Z by Duncan Haldane

main.cpp@2:aa0bdbe1fe80, 2016-09-28 (annotated)

Committer:
abuchan
Date:
Wed Sep 28 23:54:47 2016 +0000
Revision:
2:aa0bdbe1fe80
Parent:
1:d68c51a0a706 
Adapting to uses packet based UART communication.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dhaldane 0:00c56a9c6dee 1 #include "mbed.h"
abuchan 2:aa0bdbe1fe80 2
dhaldane 0:00c56a9c6dee 3 #include "enc.h"
abuchan 2:aa0bdbe1fe80 4 #include "protocol.h"
abuchan 2:aa0bdbe1fe80 5 #include "packet_parser.h"
abuchan 2:aa0bdbe1fe80 6
abuchan 2:aa0bdbe1fe80 7 #define LED1 PTC2
abuchan 2:aa0bdbe1fe80 8 #define LED2 PTC3
abuchan 2:aa0bdbe1fe80 9 #define LED3 PTA18
abuchan 2:aa0bdbe1fe80 10
abuchan 2:aa0bdbe1fe80 11 #define SERIAL_BAUDRATE 230400
abuchan 2:aa0bdbe1fe80 12 #define UART_TX PTD7
abuchan 2:aa0bdbe1fe80 13 #define UART_RX PTD6
dhaldane 0:00c56a9c6dee 14
dhaldane 1:d68c51a0a706 15 //Ticker RTI;
abuchan 2:aa0bdbe1fe80 16 //Serial imProc(UART_TX,UART_RX);
abuchan 2:aa0bdbe1fe80 17 DigitalOut led_1(LED1);
abuchan 2:aa0bdbe1fe80 18 //DigitalOut led_2(LED2);
abuchan 2:aa0bdbe1fe80 19 //DigitalOut led_3(LED3);
dhaldane 1:d68c51a0a706 20 enc motPos(PTC6,PTC7,PTC5,PTC4);
dhaldane 1:d68c51a0a706 21 PwmOut EN(PTA4);
dhaldane 1:d68c51a0a706 22 DigitalOut DR(PTA2);
dhaldane 1:d68c51a0a706 23 DigitalOut BRAKE(PTA1);
dhaldane 1:d68c51a0a706 24 AnalogIn motCurrent(PTB0);
dhaldane 1:d68c51a0a706 25 AnalogIn temp(PTE30);
abuchan 2:aa0bdbe1fe80 26 AnalogIn voltage(PTD5);
dhaldane 0:00c56a9c6dee 27
dhaldane 0:00c56a9c6dee 28 void get_state(){
abuchan 2:aa0bdbe1fe80 29 motPos.update_pos();
abuchan 2:aa0bdbe1fe80 30 }
abuchan 2:aa0bdbe1fe80 31
abuchan 2:aa0bdbe1fe80 32 void fill_sensor_packet(packet_t* pkt, int32_t position, int32_t velocity, uint16_t current, uint16_t voltage, uint16_t temperature) {
abuchan 2:aa0bdbe1fe80 33 pkt->header.type = PKT_TYPE_SENSOR;
abuchan 2:aa0bdbe1fe80 34 pkt->header.length = sizeof(header_t) + sizeof(sensor_data_t) + 1;
abuchan 2:aa0bdbe1fe80 35 sensor_data_t* sensor_data = (sensor_data_t*)pkt->data_crc;
abuchan 2:aa0bdbe1fe80 36 sensor_data->position = position;
abuchan 2:aa0bdbe1fe80 37 sensor_data->velocity = velocity;
abuchan 2:aa0bdbe1fe80 38 sensor_data->current = current;
abuchan 2:aa0bdbe1fe80 39 sensor_data->voltage = voltage;
abuchan 2:aa0bdbe1fe80 40 sensor_data->temperature = temperature;
dhaldane 0:00c56a9c6dee 41 }
dhaldane 0:00c56a9c6dee 42
dhaldane 1:d68c51a0a706 43 int main() {
abuchan 2:aa0bdbe1fe80 44 EN.period_us(50);
abuchan 2:aa0bdbe1fe80 45 EN.write(0.9f);
abuchan 2:aa0bdbe1fe80 46 DR.write(0);
abuchan 2:aa0bdbe1fe80 47 BRAKE.write(1);
abuchan 2:aa0bdbe1fe80 48 motPos.set_offset(2160);
abuchan 2:aa0bdbe1fe80 49 led_1 = 1;
abuchan 2:aa0bdbe1fe80 50 // RTI.attach(&get_state, 0.01f);
abuchan 2:aa0bdbe1fe80 51
abuchan 2:aa0bdbe1fe80 52
abuchan 2:aa0bdbe1fe80 53 Timer system_timer;
abuchan 2:aa0bdbe1fe80 54 system_timer.start();
abuchan 2:aa0bdbe1fe80 55 uint32_t last_time = system_timer.read_ms();
abuchan 2:aa0bdbe1fe80 56 uint32_t current_time = last_time;
abuchan 2:aa0bdbe1fe80 57
abuchan 2:aa0bdbe1fe80 58
abuchan 2:aa0bdbe1fe80 59 PacketParser parser(SERIAL_BAUDRATE, USBTX, USBRX, LED2, LED3);
abuchan 2:aa0bdbe1fe80 60 packet_union_t* recv_pkt = NULL;
abuchan 2:aa0bdbe1fe80 61 command_data_t* command;
abuchan 2:aa0bdbe1fe80 62
abuchan 2:aa0bdbe1fe80 63 packet_union_t* sensor_pkt = parser.get_send_packet();
abuchan 2:aa0bdbe1fe80 64
abuchan 2:aa0bdbe1fe80 65 while(1) {
abuchan 2:aa0bdbe1fe80 66 recv_pkt = parser.get_received_packet();
abuchan 2:aa0bdbe1fe80 67
abuchan 2:aa0bdbe1fe80 68 if (recv_pkt != NULL) {
abuchan 2:aa0bdbe1fe80 69
abuchan 2:aa0bdbe1fe80 70 switch (recv_pkt->packet.header.type) {
dhaldane 1:d68c51a0a706 71
abuchan 2:aa0bdbe1fe80 72 case PKT_TYPE_COMMAND:
abuchan 2:aa0bdbe1fe80 73 command = (command_data_t*)recv_pkt->packet.data_crc;
abuchan 2:aa0bdbe1fe80 74 if (sensor_pkt != NULL) {
abuchan 2:aa0bdbe1fe80 75 fill_sensor_packet(&(sensor_pkt->packet),
abuchan 2:aa0bdbe1fe80 76 motPos.ams_read(),
abuchan 2:aa0bdbe1fe80 77 0,
abuchan 2:aa0bdbe1fe80 78 motCurrent.read_u16(),
abuchan 2:aa0bdbe1fe80 79 voltage.read_u16(),
abuchan 2:aa0bdbe1fe80 80 temp.read_u16()
abuchan 2:aa0bdbe1fe80 81 );
abuchan 2:aa0bdbe1fe80 82 parser.send_packet(sensor_pkt);
abuchan 2:aa0bdbe1fe80 83 sensor_pkt = parser.get_send_packet();
abuchan 2:aa0bdbe1fe80 84 }
abuchan 2:aa0bdbe1fe80 85 break;
abuchan 2:aa0bdbe1fe80 86 }
abuchan 2:aa0bdbe1fe80 87
abuchan 2:aa0bdbe1fe80 88 parser.free_received_packet(recv_pkt);
dhaldane 0:00c56a9c6dee 89 }
abuchan 2:aa0bdbe1fe80 90
abuchan 2:aa0bdbe1fe80 91 current_time = system_timer.read_ms();
abuchan 2:aa0bdbe1fe80 92 if (current_time - last_time > 500) {
abuchan 2:aa0bdbe1fe80 93 last_time = current_time;
abuchan 2:aa0bdbe1fe80 94 led_1 = !led_1;
abuchan 2:aa0bdbe1fe80 95 }
abuchan 2:aa0bdbe1fe80 96
abuchan 2:aa0bdbe1fe80 97 Thread::yield();
abuchan 2:aa0bdbe1fe80 98 }
dhaldane 0:00c56a9c6dee 99 }