Initial Commit
Dependencies: mbed HC05 QEI MODSERIAL SWSPI mbed-rtos
main.cpp@6:5ab1735265a9, 2014-10-21 (annotated)
- Committer:
- harshb
- Date:
- Tue Oct 21 21:59:15 2014 +0000
- Revision:
- 6:5ab1735265a9
- Parent:
- 5:eb706d3e512c
Initial Commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Throwbot | 1:1da89c13dfa1 | 1 | // FORMAT_CODE_START |
Throwbot | 0:43331220df0d | 2 | #include "QEI.h" |
Throwbot | 0:43331220df0d | 3 | #include "mbed.h" |
Throwbot | 0:43331220df0d | 4 | #include "robot.h" |
Throwbot | 0:43331220df0d | 5 | #include "rtos.h" |
Throwbot | 1:1da89c13dfa1 | 6 | #include "MPU6050.h" |
Throwbot | 1:1da89c13dfa1 | 7 | #include "I2Cdev.h" |
Throwbot | 1:1da89c13dfa1 | 8 | #include "tone.h" |
Throwbot | 1:1da89c13dfa1 | 9 | #include "ultrasonic.h" |
Throwbot | 2:0f80c8a1c236 | 10 | #include "bt_shell.h" |
Throwbot | 4:81b0de07841f | 11 | #include "bt_shell_f.h" |
Throwbot | 2:0f80c8a1c236 | 12 | int Current_Right_pulse=0; |
Throwbot | 2:0f80c8a1c236 | 13 | int Current_Left_pulse=0; |
Throwbot | 2:0f80c8a1c236 | 14 | int Linput=0; |
Throwbot | 2:0f80c8a1c236 | 15 | int Rinput=0; |
Throwbot | 2:0f80c8a1c236 | 16 | int Change_Right_pulse=0; |
Throwbot | 2:0f80c8a1c236 | 17 | int Change_Left_pulse=0; |
Throwbot | 2:0f80c8a1c236 | 18 | int Previous_Left_pulse=0; |
Throwbot | 2:0f80c8a1c236 | 19 | int Previos_Right_pulse=0; |
Throwbot | 2:0f80c8a1c236 | 20 | int time_int = 50; |
Throwbot | 2:0f80c8a1c236 | 21 | int time_factor=1000/time_int ; |
Throwbot | 3:d1197b5ea68a | 22 | int Error_Right=0; |
Throwbot | 3:d1197b5ea68a | 23 | int Error_Left=0; |
Throwbot | 3:d1197b5ea68a | 24 | float L_Kp=0.1; |
Throwbot | 3:d1197b5ea68a | 25 | float R_Kp=0.1; |
Throwbot | 4:81b0de07841f | 26 | int Last_Error_Left=0; |
Throwbot | 4:81b0de07841f | 27 | int Last_Error_Right=0; |
Throwbot | 3:d1197b5ea68a | 28 | int previous_Linput= 0 ; |
Throwbot | 3:d1197b5ea68a | 29 | int previous_Rinput= 0 ; |
Throwbot | 2:0f80c8a1c236 | 30 | RtosTimer *Motor_controller_Timer; |
Throwbot | 2:0f80c8a1c236 | 31 | |
Throwbot | 2:0f80c8a1c236 | 32 | void Motorcontroller(void const *args) |
Throwbot | 2:0f80c8a1c236 | 33 | { |
Throwbot | 2:0f80c8a1c236 | 34 | |
harshb | 6:5ab1735265a9 | 35 | Current_Right_pulse= right.getPulses()/10; |
harshb | 6:5ab1735265a9 | 36 | Current_Left_pulse=left.getPulses()*(-1)/9.85; |
Throwbot | 2:0f80c8a1c236 | 37 | |
Throwbot | 2:0f80c8a1c236 | 38 | Change_Right_pulse=Current_Right_pulse- Previos_Right_pulse; |
Throwbot | 2:0f80c8a1c236 | 39 | Change_Left_pulse=Current_Left_pulse- Previous_Left_pulse; |
Throwbot | 2:0f80c8a1c236 | 40 | |
Throwbot | 2:0f80c8a1c236 | 41 | Previous_Left_pulse=Current_Left_pulse; |
Throwbot | 2:0f80c8a1c236 | 42 | Previos_Right_pulse=Current_Right_pulse; |
Throwbot | 2:0f80c8a1c236 | 43 | |
Throwbot | 3:d1197b5ea68a | 44 | Error_Right=(Rmotor_speed- (Change_Right_pulse*time_factor) ); |
Throwbot | 3:d1197b5ea68a | 45 | Error_Left=(Lmotor_speed- (Change_Left_pulse*time_factor)); |
Throwbot | 2:0f80c8a1c236 | 46 | Linput=(Linput+Error_Left*L_Kp); |
Throwbot | 2:0f80c8a1c236 | 47 | Rinput=(Rinput+Error_Right*R_Kp); |
Throwbot | 2:0f80c8a1c236 | 48 | if(Linput>100) |
Throwbot | 2:0f80c8a1c236 | 49 | Linput=100; |
Throwbot | 2:0f80c8a1c236 | 50 | else if (Linput<-100) |
Throwbot | 2:0f80c8a1c236 | 51 | Linput= -100; |
Throwbot | 2:0f80c8a1c236 | 52 | if(Rinput>100) |
Throwbot | 2:0f80c8a1c236 | 53 | Rinput=100; |
Throwbot | 2:0f80c8a1c236 | 54 | else if(Rinput<-100) |
Throwbot | 2:0f80c8a1c236 | 55 | Rinput= -100; |
Throwbot | 4:81b0de07841f | 56 | if(Error_Right==0) { |
Throwbot | 4:81b0de07841f | 57 | Last_Error_Right++; |
Throwbot | 3:d1197b5ea68a | 58 | } else { |
Throwbot | 4:81b0de07841f | 59 | Last_Error_Right=0; |
Throwbot | 4:81b0de07841f | 60 | } |
Throwbot | 4:81b0de07841f | 61 | if(Error_Left==0) { |
Throwbot | 4:81b0de07841f | 62 | Last_Error_Left++; |
Throwbot | 4:81b0de07841f | 63 | } else { |
Throwbot | 4:81b0de07841f | 64 | Last_Error_Left=0; |
Throwbot | 3:d1197b5ea68a | 65 | } |
Throwbot | 4:81b0de07841f | 66 | if(Last_Error_Right==100) { |
Throwbot | 4:81b0de07841f | 67 | Rinput=0; |
Throwbot | 4:81b0de07841f | 68 | } |
Throwbot | 4:81b0de07841f | 69 | if(Last_Error_Left==100) { |
Throwbot | 4:81b0de07841f | 70 | Linput=0; |
Throwbot | 4:81b0de07841f | 71 | } |
harshb | 6:5ab1735265a9 | 72 | *motors.Left = Linput; // HARSH change to -Linput for black and Linput for blue |
harshb | 6:5ab1735265a9 | 73 | *motors.Right = -Rinput; //// HARSH change to Rinput for black and -Rinput for blue |
Throwbot | 2:0f80c8a1c236 | 74 | } |
Throwbot | 0:43331220df0d | 75 | char c; |
Throwbot | 1:1da89c13dfa1 | 76 | char buffer[10]; |
Throwbot | 2:0f80c8a1c236 | 77 | void bt_shell_thr(void const *args) |
Throwbot | 2:0f80c8a1c236 | 78 | { |
Throwbot | 2:0f80c8a1c236 | 79 | while(true) { |
Throwbot | 4:81b0de07841f | 80 | if(Selected_robot=='A') { |
Throwbot | 4:81b0de07841f | 81 | bt_shell_run(); |
Throwbot | 4:81b0de07841f | 82 | } else if(Selected_robot=='F') { |
Throwbot | 4:81b0de07841f | 83 | bt_shell_f_run(); |
Throwbot | 4:81b0de07841f | 84 | } |
Throwbot | 2:0f80c8a1c236 | 85 | Thread::wait(50); |
Throwbot | 2:0f80c8a1c236 | 86 | } |
Throwbot | 2:0f80c8a1c236 | 87 | } |
Throwbot | 1:1da89c13dfa1 | 88 | |
Throwbot | 1:1da89c13dfa1 | 89 | int main() |
Throwbot | 1:1da89c13dfa1 | 90 | { |
Throwbot | 1:1da89c13dfa1 | 91 | initRobot(); |
Throwbot | 1:1da89c13dfa1 | 92 | while(buffer[3] != 'O') { |
Throwbot | 1:1da89c13dfa1 | 93 | while(buffer[3] != 'E') { |
Throwbot | 1:1da89c13dfa1 | 94 | if(bt.readable()) { |
Throwbot | 1:1da89c13dfa1 | 95 | bt.gets(buffer, 5); |
Throwbot | 1:1da89c13dfa1 | 96 | if(buffer[3] == 'E') { |
Throwbot | 1:1da89c13dfa1 | 97 | bt.printf(">>1B"); |
Throwbot | 1:1da89c13dfa1 | 98 | } else if(buffer[3] == '?') { |
Throwbot | 2:0f80c8a1c236 | 99 | bt.printf("eBot#2"); |
Throwbot | 1:1da89c13dfa1 | 100 | } |
Throwbot | 1:1da89c13dfa1 | 101 | } |
Throwbot | 1:1da89c13dfa1 | 102 | } |
Throwbot | 1:1da89c13dfa1 | 103 | if(bt.readable()) { |
Throwbot | 1:1da89c13dfa1 | 104 | bt.gets(buffer, 5); |
Throwbot | 1:1da89c13dfa1 | 105 | } |
Throwbot | 1:1da89c13dfa1 | 106 | } |
Throwbot | 4:81b0de07841f | 107 | bt_connected=true; |
Throwbot | 4:81b0de07841f | 108 | bt.gets(buffer, 2); |
Throwbot | 4:81b0de07841f | 109 | if(buffer[0]=='A') { |
Throwbot | 4:81b0de07841f | 110 | Selected_robot='A'; |
Throwbot | 5:eb706d3e512c | 111 | //imperial_march(); |
Throwbot | 4:81b0de07841f | 112 | |
Throwbot | 4:81b0de07841f | 113 | } else if(buffer[0]=='F') { |
Throwbot | 4:81b0de07841f | 114 | Selected_robot='F'; |
Throwbot | 4:81b0de07841f | 115 | Led_on(); |
Throwbot | 4:81b0de07841f | 116 | } |
Throwbot | 4:81b0de07841f | 117 | Thread bt_shell_th(bt_shell_thr, NULL, osPriorityNormal, 2048, NULL); |
Throwbot | 1:1da89c13dfa1 | 118 | Thread IMU_th(Imu_yaw, NULL, osPriorityNormal, 2048, NULL); |
Throwbot | 1:1da89c13dfa1 | 119 | Thread Encoder_th(encoder_thread, NULL, osPriorityNormal, 2048, NULL); |
Throwbot | 2:0f80c8a1c236 | 120 | Motor_controller_Timer = new RtosTimer(&Motorcontroller, osTimerPeriodic, NULL); |
Throwbot | 3:d1197b5ea68a | 121 | Motor_controller_Timer->start(time_int); |
Throwbot | 1:1da89c13dfa1 | 122 | while(1) { |
Throwbot | 4:81b0de07841f | 123 | if(Selected_robot=='A') { |
Throwbot | 4:81b0de07841f | 124 | if(ldrread2()>0.6) { |
Throwbot | 4:81b0de07841f | 125 | Led_on(); |
Throwbot | 4:81b0de07841f | 126 | } else { |
Throwbot | 4:81b0de07841f | 127 | Led_off(); |
Throwbot | 4:81b0de07841f | 128 | } |
Throwbot | 1:1da89c13dfa1 | 129 | } |
Throwbot | 2:0f80c8a1c236 | 130 | ultrasonic_run(); |
Throwbot | 2:0f80c8a1c236 | 131 | Thread::yield(); |
Throwbot | 0:43331220df0d | 132 | } |
Throwbot | 2:0f80c8a1c236 | 133 | } |