Oscar Liao / Mbed 2 deprecated CHICAGO_test

Original

Dependencies:   mbed

main.cpp@1:74aad7c18ccf, 2018-12-21 (annotated)

Committer:
OscarLiao
Date:
Fri Dec 21 02:25:54 2018 +0000
Revision:
1:74aad7c18ccf
Parent:
0:65d0d8265089 
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
OscarLiao 0:65d0d8265089 1 /*
OscarLiao 0:65d0d8265089 2 This is a program for globlelized controller of whole DOGO
OscarLiao 0:65d0d8265089 3 F746ZG
OscarLiao 0:65d0d8265089 4 */
OscarLiao 0:65d0d8265089 5
OscarLiao 0:65d0d8265089 6 #include "mbed.h"
OscarLiao 0:65d0d8265089 7 //#include "LSM9DS0_SH.h"
OscarLiao 0:65d0d8265089 8 #define DEBUG 0
OscarLiao 0:65d0d8265089 9
OscarLiao 0:65d0d8265089 10 #define pi 3.141592f
OscarLiao 0:65d0d8265089 11 #define d2r 0.01745329f
OscarLiao 0:65d0d8265089 12
OscarLiao 0:65d0d8265089 13 #define Rms 5000 //TT rate
OscarLiao 0:65d0d8265089 14 #define dt 0.015f
OscarLiao 0:65d0d8265089 15 #define Task_1_NN 120
OscarLiao 0:65d0d8265089 16 #define Task_2_NN 5
OscarLiao 1:74aad7c18ccf 17 #define Task_4_NN 9
OscarLiao 0:65d0d8265089 18
OscarLiao 0:65d0d8265089 19 //Structure
OscarLiao 0:65d0d8265089 20 #define Buff_size 16 //Serial Buffer
OscarLiao 0:65d0d8265089 21
OscarLiao 1:74aad7c18ccf 22 //Initial Position
OscarLiao 1:74aad7c18ccf 23 #define init_leg_Angle1 0
OscarLiao 1:74aad7c18ccf 24 #define init_leg_length 0.22f
OscarLiao 1:74aad7c18ccf 25 #define init_leg_Angle2 0
OscarLiao 1:74aad7c18ccf 26
OscarLiao 0:65d0d8265089 27 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
OscarLiao 0:65d0d8265089 28
OscarLiao 0:65d0d8265089 29 //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡GPIO registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
OscarLiao 0:65d0d8265089 30 //╔═════════════════╗
OscarLiao 0:65d0d8265089 31 //║ Structure ║
OscarLiao 0:65d0d8265089 32 //╚═════════════════╝
OscarLiao 0:65d0d8265089 33 DigitalOut LD1(PB_0); //detection
OscarLiao 0:65d0d8265089 34 DigitalOut LD2(PB_7); //detection
OscarLiao 0:65d0d8265089 35 DigitalOut LD3(PB_14); //detection
OscarLiao 0:65d0d8265089 36 InterruptIn button(USER_BUTTON); //Button press;
OscarLiao 0:65d0d8265089 37
OscarLiao 0:65d0d8265089 38 //╔═════════════════╗
OscarLiao 0:65d0d8265089 39 //║ Serial ║
OscarLiao 0:65d0d8265089 40 //╚═════════════════╝
OscarLiao 0:65d0d8265089 41 Serial Debug(PD_8, PD_9); //Serial_3 reg(TX RX) USB port
OscarLiao 0:65d0d8265089 42
OscarLiao 0:65d0d8265089 43 Serial LF_Cmd(PD_5, PD_6); //Serial_2
OscarLiao 0:65d0d8265089 44 DigitalOut LF_CS(PD_7);
OscarLiao 0:65d0d8265089 45
OscarLiao 0:65d0d8265089 46 Serial LH_Cmd(PC_12, PD_2); //Serial_5
OscarLiao 0:65d0d8265089 47 DigitalOut LH_CS(PG_2);
OscarLiao 0:65d0d8265089 48
OscarLiao 0:65d0d8265089 49 Serial RF_Cmd(PG_14, PC_7); //Serial_6
OscarLiao 1:74aad7c18ccf 50 DigitalOut RF_CS(PG_9);
OscarLiao 0:65d0d8265089 51
OscarLiao 0:65d0d8265089 52 Serial RH_Cmd(PE_8, PE_7); //Serial_7
OscarLiao 1:74aad7c18ccf 53 DigitalOut RH_CS(PE_10);
OscarLiao 0:65d0d8265089 54 //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of GPIO registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
OscarLiao 0:65d0d8265089 55
OscarLiao 0:65d0d8265089 56
OscarLiao 0:65d0d8265089 57 //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Varible registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
OscarLiao 0:65d0d8265089 58 //╔═════════════════╗
OscarLiao 0:65d0d8265089 59 //║ Structure ║
OscarLiao 0:65d0d8265089 60 //╚═════════════════╝
OscarLiao 0:65d0d8265089 61 Ticker TT; //call a timer
OscarLiao 0:65d0d8265089 62 uint8_t Task_1_count = 0; //1st prior task count
OscarLiao 0:65d0d8265089 63 uint8_t Task_2_count = 0; //2nd prior task count
OscarLiao 0:65d0d8265089 64 uint8_t Task_3_count = 0; //3nd prior task count
OscarLiao 0:65d0d8265089 65 uint8_t Task_4_count = 0; //24nd prior task count
OscarLiao 0:65d0d8265089 66 uint8_t Flag_1 = 0; //1st prior task flag
OscarLiao 0:65d0d8265089 67 uint8_t Flag_2 = 0; //2nd prior task flag
OscarLiao 0:65d0d8265089 68 uint8_t Flag_3 = 0; //3nd prior task flag
OscarLiao 0:65d0d8265089 69 uint8_t Flag_4 = 0; //4nd prior task flag
OscarLiao 0:65d0d8265089 70
OscarLiao 0:65d0d8265089 71
OscarLiao 0:65d0d8265089 72 //╔═════════════════╗
OscarLiao 0:65d0d8265089 73 //║ I/O Serial ║
OscarLiao 0:65d0d8265089 74 //╚═════════════════╝
OscarLiao 0:65d0d8265089 75 uint8_t Buff[Buff_size];
OscarLiao 0:65d0d8265089 76 uint8_t Recieve_index = 0;
OscarLiao 0:65d0d8265089 77
OscarLiao 0:65d0d8265089 78
OscarLiao 1:74aad7c18ccf 79 //╔═════════════════╗
OscarLiao 1:74aad7c18ccf 80 //║ Leg Command ║
OscarLiao 1:74aad7c18ccf 81 //╚═════════════════╝
OscarLiao 1:74aad7c18ccf 82
OscarLiao 1:74aad7c18ccf 83 float LF_q_0_E[3] = {
OscarLiao 1:74aad7c18ccf 84 init_leg_Angle1, init_leg_length, init_leg_Angle2
OscarLiao 1:74aad7c18ccf 85 };
OscarLiao 0:65d0d8265089 86 float LH_q_0_E[3] = {
OscarLiao 1:74aad7c18ccf 87 init_leg_Angle1, init_leg_length, init_leg_Angle2
OscarLiao 1:74aad7c18ccf 88 };
OscarLiao 1:74aad7c18ccf 89 float RF_q_0_E[3] = {
OscarLiao 1:74aad7c18ccf 90 init_leg_Angle1, init_leg_length, init_leg_Angle2
OscarLiao 1:74aad7c18ccf 91 };
OscarLiao 1:74aad7c18ccf 92 float RH_q_0_E[3] = {
OscarLiao 1:74aad7c18ccf 93 init_leg_Angle1, init_leg_length, init_leg_Angle2
OscarLiao 0:65d0d8265089 94 };
OscarLiao 0:65d0d8265089 95
OscarLiao 1:74aad7c18ccf 96 uint8_t LF_Cmd_Hex[3] = {0x00, 0x00, 0x00};
OscarLiao 1:74aad7c18ccf 97 uint8_t LH_Cmd_Hex[3] = {0x00, 0x00, 0x00};
OscarLiao 1:74aad7c18ccf 98 uint8_t RF_Cmd_Hex[3] = {0x00, 0x00, 0x00};
OscarLiao 1:74aad7c18ccf 99 uint8_t RH_Cmd_Hex[3] = {0x00, 0x00, 0x00};
OscarLiao 0:65d0d8265089 100
OscarLiao 0:65d0d8265089 101
OscarLiao 0:65d0d8265089 102 //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Varible registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
OscarLiao 0:65d0d8265089 103
OscarLiao 0:65d0d8265089 104
OscarLiao 0:65d0d8265089 105 //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Function registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
OscarLiao 0:65d0d8265089 106 void init_TIMER(); //set TT_main() rate
OscarLiao 0:65d0d8265089 107 void TT_main(); //timebase function rated by TT
OscarLiao 0:65d0d8265089 108 void Rx_irq();
OscarLiao 0:65d0d8265089 109 //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Function registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
OscarLiao 0:65d0d8265089 110
OscarLiao 0:65d0d8265089 111
OscarLiao 0:65d0d8265089 112 //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡main funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
OscarLiao 0:65d0d8265089 113 int main()
OscarLiao 0:65d0d8265089 114 {
OscarLiao 0:65d0d8265089 115 Debug.baud(115200); //set baud rate
OscarLiao 0:65d0d8265089 116 LF_Cmd.baud(115200);
OscarLiao 0:65d0d8265089 117 LH_Cmd.baud(115200);
OscarLiao 0:65d0d8265089 118 RF_Cmd.baud(115200);
OscarLiao 0:65d0d8265089 119 RH_Cmd.baud(115200);
OscarLiao 0:65d0d8265089 120
OscarLiao 0:65d0d8265089 121 wait_ms(10);
OscarLiao 0:65d0d8265089 122 init_TIMER(); //start TT_main
OscarLiao 0:65d0d8265089 123
OscarLiao 0:65d0d8265089 124 Debug.attach(&Rx_irq,Serial::RxIrq); //Start recieving message
OscarLiao 0:65d0d8265089 125
OscarLiao 0:65d0d8265089 126 LH_CS = 1;
OscarLiao 0:65d0d8265089 127 wait_ms(200);
OscarLiao 0:65d0d8265089 128
OscarLiao 1:74aad7c18ccf 129
OscarLiao 0:65d0d8265089 130
OscarLiao 0:65d0d8265089 131 while(1) { //main() loop
OscarLiao 0:65d0d8265089 132
OscarLiao 0:65d0d8265089 133 // if(Debug.readable()) {
OscarLiao 0:65d0d8265089 134 // Buff[Recieve_index] = Debug.getc();
OscarLiao 0:65d0d8265089 135 // Debug.putc(Buff[Recieve_index]);
OscarLiao 0:65d0d8265089 136 // Recieve_index = Recieve_index + 1;
OscarLiao 0:65d0d8265089 137 // }
OscarLiao 0:65d0d8265089 138
OscarLiao 0:65d0d8265089 139 // if(device.readable()) {
OscarLiao 0:65d0d8265089 140 // pc.putc(device.getc());
OscarLiao 0:65d0d8265089 141 // }
OscarLiao 1:74aad7c18ccf 142
OscarLiao 1:74aad7c18ccf 143 //Send commands
OscarLiao 1:74aad7c18ccf 144 if(Flag_4 == 1) {
OscarLiao 1:74aad7c18ccf 145
OscarLiao 1:74aad7c18ccf 146 LF_Cmd_Hex[0] = (LF_q_0_E[0]+pi/4)/(pi/2)*255;
OscarLiao 1:74aad7c18ccf 147 LF_Cmd_Hex[1] = ((LF_q_0_E[1]-0.1)/0.2)*255;
OscarLiao 1:74aad7c18ccf 148 LF_Cmd_Hex[2] = (LF_q_0_E[2]+pi/4)/(pi/2)*255;
OscarLiao 1:74aad7c18ccf 149
OscarLiao 1:74aad7c18ccf 150 LH_Cmd_Hex[0] = (LH_q_0_E[0]+pi/4)/(pi/2)*255;
OscarLiao 1:74aad7c18ccf 151 LH_Cmd_Hex[1] = ((LH_q_0_E[1]-0.1)/0.2)*255;
OscarLiao 1:74aad7c18ccf 152 LH_Cmd_Hex[2] = (LH_q_0_E[2]+pi/4)/(pi/2)*255;
OscarLiao 1:74aad7c18ccf 153
OscarLiao 1:74aad7c18ccf 154 RF_Cmd_Hex[0] = (RF_q_0_E[0]+pi/4)/(pi/2)*255;
OscarLiao 1:74aad7c18ccf 155 RF_Cmd_Hex[1] = ((RF_q_0_E[1]-0.1)/0.2)*255;
OscarLiao 1:74aad7c18ccf 156 RF_Cmd_Hex[2] = (RF_q_0_E[2]+pi/4)/(pi/2)*255;
OscarLiao 1:74aad7c18ccf 157
OscarLiao 1:74aad7c18ccf 158 RH_Cmd_Hex[0] = (RH_q_0_E[0]+pi/4)/(pi/2)*255;
OscarLiao 1:74aad7c18ccf 159 RH_Cmd_Hex[1] = ((RH_q_0_E[1]-0.1)/0.2)*255;
OscarLiao 1:74aad7c18ccf 160 RH_Cmd_Hex[2] = (RH_q_0_E[2]+pi/4)/(pi/2)*255;
OscarLiao 1:74aad7c18ccf 161
OscarLiao 1:74aad7c18ccf 162 //Debug.printf("%d, %d, %d\r", LH_Cmd_Hex[0], LH_Cmd_Hex[1], LH_Cmd_Hex[2]);
OscarLiao 1:74aad7c18ccf 163 //LED
OscarLiao 0:65d0d8265089 164 LD1 = 1;
OscarLiao 1:74aad7c18ccf 165
OscarLiao 1:74aad7c18ccf 166 //Left Front Leg
OscarLiao 1:74aad7c18ccf 167 LF_CS = 0;
OscarLiao 1:74aad7c18ccf 168 wait_us(20);
OscarLiao 1:74aad7c18ccf 169 LF_Cmd.putc(LF_Cmd_Hex[0]);
OscarLiao 1:74aad7c18ccf 170 LF_Cmd.putc(LF_Cmd_Hex[1]);
OscarLiao 1:74aad7c18ccf 171 LF_Cmd.putc(LF_Cmd_Hex[2]);
OscarLiao 1:74aad7c18ccf 172 LF_Cmd.putc('Q');
OscarLiao 1:74aad7c18ccf 173 wait_us(180);
OscarLiao 1:74aad7c18ccf 174 LF_CS = 1;
OscarLiao 1:74aad7c18ccf 175
OscarLiao 1:74aad7c18ccf 176 //Left Hind Leg
OscarLiao 0:65d0d8265089 177 LH_CS = 0;
OscarLiao 0:65d0d8265089 178 wait_us(20);
OscarLiao 1:74aad7c18ccf 179 LH_Cmd.putc(LH_Cmd_Hex[0]);
OscarLiao 1:74aad7c18ccf 180 LH_Cmd.putc(LH_Cmd_Hex[1]);
OscarLiao 1:74aad7c18ccf 181 LH_Cmd.putc(LH_Cmd_Hex[2]);
OscarLiao 0:65d0d8265089 182 LH_Cmd.putc('Q');
OscarLiao 0:65d0d8265089 183 wait_us(180);
OscarLiao 0:65d0d8265089 184 LH_CS = 1;
OscarLiao 1:74aad7c18ccf 185
OscarLiao 1:74aad7c18ccf 186 //Right Front Leg
OscarLiao 1:74aad7c18ccf 187 RF_CS = 0;
OscarLiao 1:74aad7c18ccf 188 wait_us(20);
OscarLiao 1:74aad7c18ccf 189 RF_Cmd.putc(RF_Cmd_Hex[0]);
OscarLiao 1:74aad7c18ccf 190 RF_Cmd.putc(RF_Cmd_Hex[1]);
OscarLiao 1:74aad7c18ccf 191 RF_Cmd.putc(RF_Cmd_Hex[2]);
OscarLiao 1:74aad7c18ccf 192 RF_Cmd.putc('Q');
OscarLiao 1:74aad7c18ccf 193 wait_us(180);
OscarLiao 1:74aad7c18ccf 194 RF_CS = 1;
OscarLiao 1:74aad7c18ccf 195
OscarLiao 1:74aad7c18ccf 196 //Right Hind Leg
OscarLiao 1:74aad7c18ccf 197 RH_CS = 0;
OscarLiao 1:74aad7c18ccf 198 wait_us(20);
OscarLiao 1:74aad7c18ccf 199 RH_Cmd.putc(RH_Cmd_Hex[0]);
OscarLiao 1:74aad7c18ccf 200 RH_Cmd.putc(RH_Cmd_Hex[1]);
OscarLiao 1:74aad7c18ccf 201 RH_Cmd.putc(RH_Cmd_Hex[2]);
OscarLiao 1:74aad7c18ccf 202 RH_Cmd.putc('Q');
OscarLiao 1:74aad7c18ccf 203 wait_us(180);
OscarLiao 1:74aad7c18ccf 204 RH_CS = 1;
OscarLiao 1:74aad7c18ccf 205
OscarLiao 0:65d0d8265089 206 LD1 = 0;
OscarLiao 0:65d0d8265089 207
OscarLiao 1:74aad7c18ccf 208 Flag_4 = 0; //clear pending
OscarLiao 0:65d0d8265089 209 }
OscarLiao 0:65d0d8265089 210 }
OscarLiao 0:65d0d8265089 211 }
OscarLiao 0:65d0d8265089 212 //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of main funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
OscarLiao 0:65d0d8265089 213
OscarLiao 0:65d0d8265089 214
OscarLiao 0:65d0d8265089 215 //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Timebase funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
OscarLiao 0:65d0d8265089 216 void init_TIMER() //set TT_main{} rate
OscarLiao 0:65d0d8265089 217 {
OscarLiao 0:65d0d8265089 218 TT.attach_us(&TT_main, Rms);
OscarLiao 0:65d0d8265089 219 }
OscarLiao 0:65d0d8265089 220 void TT_main() //interrupt function by TT
OscarLiao 0:65d0d8265089 221 {
OscarLiao 0:65d0d8265089 222 Task_1_count = Task_1_count + 1;
OscarLiao 0:65d0d8265089 223 if(Task_1_count > Task_1_NN) {
OscarLiao 0:65d0d8265089 224 Task_1_count = 0; //Task triggering
OscarLiao 0:65d0d8265089 225 Flag_1 = 1;
OscarLiao 1:74aad7c18ccf 226 }
OscarLiao 1:74aad7c18ccf 227 Task_4_count = Task_4_count + 1;
OscarLiao 1:74aad7c18ccf 228 if(Task_4_count > Task_4_NN) {
OscarLiao 1:74aad7c18ccf 229 Task_4_count = 0; //Task triggering
OscarLiao 1:74aad7c18ccf 230 Flag_4 = 1;
OscarLiao 0:65d0d8265089 231 }
OscarLiao 0:65d0d8265089 232 }
OscarLiao 0:65d0d8265089 233 //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Timebase funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
OscarLiao 0:65d0d8265089 234
OscarLiao 0:65d0d8265089 235
OscarLiao 0:65d0d8265089 236 //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Rx_irq funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
OscarLiao 0:65d0d8265089 237 void Rx_irq(void)
OscarLiao 0:65d0d8265089 238 {
OscarLiao 0:65d0d8265089 239 Buff[0] = Debug.getc();
OscarLiao 0:65d0d8265089 240 Debug.putc(Buff[0]);
OscarLiao 0:65d0d8265089 241 }
OscarLiao 0:65d0d8265089 242 //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Rx_irq funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//