AES-2015 / Mbed 2 deprecated Finalrobot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-rtos mbed

main.cpp@2:d7cd4deef7be, 2015-12-15 (annotated)

Committer:
BAC
Date:
Tue Dec 15 02:42:41 2015 +0000
Revision:
2:d7cd4deef7be
Parent:
0:f15aa1706e16 
the newest

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BAC 0:f15aa1706e16 1 #include "mbed.h" // I/O, Math library etc.
BAC 0:f15aa1706e16 2 #include "rtos.h" // Real time OS library
BAC 0:f15aa1706e16 3 #include "trajectory.h" // Trajectory of circle
BAC 0:f15aa1706e16 4 #include "robot.h" // Kinematic of uArm
BAC 0:f15aa1706e16 5 #include "pid.h" // Close loop control function
BAC 0:f15aa1706e16 6 #include "pwm.h" // Set period and dutycycle for each servo motor
BAC 0:f15aa1706e16 7 #include "a2d.h" // A/D convert from voltage to angle
BAC 0:f15aa1706e16 8
BAC 0:f15aa1706e16 9 unsigned int i = 100; // Data index
BAC 0:f15aa1706e16 10 unsigned int j = 0;
BAC 2:d7cd4deef7be 11 unsigned int num = 37;
BAC 0:f15aa1706e16 12 float v1,v2,v3,v1avg,v2avg,v3avg;
BAC 0:f15aa1706e16 13 Serial UART(SERIAL_TX, SERIAL_RX); // Define object "UART" as Serial type
BAC 0:f15aa1706e16 14 MotorDriver Mset1; // Initial PWM period || arg(Link1,Link2,Link3,Link4)
BAC 0:f15aa1706e16 15 Sensor Sen1;
BAC 0:f15aa1706e16 16
BAC 0:f15aa1706e16 17 void state_thread(void const *args); // Thread for change inder every 1 second
BAC 0:f15aa1706e16 18 void read_thread(void const *args); // Thread for change read every 1 second
BAC 0:f15aa1706e16 19 void drive_thread(void const *args); // Thread for change read every 1 second
BAC 0:f15aa1706e16 20
BAC 0:f15aa1706e16 21 int main() {
BAC 0:f15aa1706e16 22
BAC 0:f15aa1706e16 23 UART.baud(9600); // Set BuadRate 9600 bps
BAC 0:f15aa1706e16 24 /*
BAC 0:f15aa1706e16 25 SetCircle circle1(50.0f,250.0f,0.0f,num); // Initial Circle trajectory || arg(radius,OriginX,OriginY,Number of data point)
BAC 0:f15aa1706e16 26 Kinematic uARM1(16.0f,81.0f,148.0f,155.0f,55.0f,45.0f); // Initial Dimension of uARM robot || arg(T1X,T1Z,T2,T3,T4X,T4Z)
BAC 0:f15aa1706e16 27 for(j=0;j<num;j++)
BAC 0:f15aa1706e16 28 {
BAC 0:f15aa1706e16 29 uARM1.inverse(circle1.GetX(j),circle1.GetY(j),circle1.GetZ(j),j);
BAC 0:f15aa1706e16 30 printf("A[%d] = %f, B[%d] = %f, C[%d] = %f\r",j,uARM1.GetA(j),j,uARM1.GetB(j),j,uARM1.GetC(j)); // Test
BAC 0:f15aa1706e16 31 }
BAC 0:f15aa1706e16 32 j= 0;
BAC 0:f15aa1706e16 33
BAC 0:f15aa1706e16 34 PIDcontroller PID1(0,0,0,0.04), PID2(0,0,0,0.04), PID3(0,0,0,0.04); // Initial 3 PID controller for each motor
BAC 0:f15aa1706e16 35 */
BAC 0:f15aa1706e16 36
BAC 0:f15aa1706e16 37 Thread thread_count(state_thread,NULL,osPriorityNormal,1024,NULL);
BAC 0:f15aa1706e16 38 //Thread thread_read(read_thread,NULL,osPriorityNormal,1024,NULL);
BAC 0:f15aa1706e16 39 Thread thread_drive(drive_thread,NULL,osPriorityNormal,1024,NULL);
BAC 0:f15aa1706e16 40
BAC 0:f15aa1706e16 41 while(1) {
BAC 0:f15aa1706e16 42 //convert to angle (A B C)
BAC 0:f15aa1706e16 43 //uARM1.inverse(circle1.GetX(i),circle1.GetY(i),circle1.GetZ(i));
BAC 0:f15aa1706e16 44 // Mapping to duty cycle (D1 D2 D3) period 20,000 us applicable 0-180 -> 0 - 2,000
BAC 0:f15aa1706e16 45 //Mset1.Angle2Duty(uARM1.GetA(),uARM1.GetB(),uARM1.GetC());
BAC 0:f15aa1706e16 46 //printf("\rShow A = %f, B = %f, C = %f\r",uARM1.GetA(),uARM1.GetB(),uARM1.GetC()); // Test
BAC 0:f15aa1706e16 47 // Drive
BAC 0:f15aa1706e16 48 //printf("\rShow D1 = %d, D2 = %d, D3 = %d\r",Mset1.GetD1(),Mset1.GetD2(),Mset1.GetD3()); // Test
BAC 0:f15aa1706e16 49
BAC 0:f15aa1706e16 50 /*
BAC 0:f15aa1706e16 51 Mset1.Angle2Duty(110,150,75);
BAC 0:f15aa1706e16 52 Mset1.Actuate();
BAC 0:f15aa1706e16 53 wait(2);
BAC 0:f15aa1706e16 54 Mset1.Angle2Duty(110,120,75);
BAC 0:f15aa1706e16 55 Mset1.Actuate();
BAC 0:f15aa1706e16 56 wait(2);
BAC 0:f15aa1706e16 57 Mset1.Angle2Duty(110,180,75);
BAC 0:f15aa1706e16 58 Mset1.Actuate();
BAC 0:f15aa1706e16 59 wait(2);
BAC 0:f15aa1706e16 60 */
BAC 0:f15aa1706e16 61
BAC 0:f15aa1706e16 62 //Mset1.Angle2Duty(100,100,100);
BAC 0:f15aa1706e16 63 //Mset1.Actuate();
BAC 0:f15aa1706e16 64 //wait(2);
BAC 0:f15aa1706e16 65
BAC 0:f15aa1706e16 66 //Mset1.DirectDrive(1025,1500,570);
BAC 0:f15aa1706e16 67 //wait(0.5f);
BAC 0:f15aa1706e16 68
BAC 0:f15aa1706e16 69 //printf("\rShow X%d = %f, Y%d = %f, Z%d = %f\r",i,circle1.GetX(i),i,circle1.GetY(i),i,circle1.GetZ(i)); // Test
BAC 0:f15aa1706e16 70 //wait(0.5f);
BAC 0:f15aa1706e16 71 }
BAC 0:f15aa1706e16 72 }
BAC 0:f15aa1706e16 73
BAC 0:f15aa1706e16 74 void state_thread(void const *args){
BAC 0:f15aa1706e16 75 while(1)
BAC 0:f15aa1706e16 76 {
BAC 0:f15aa1706e16 77 if(i<num){
BAC 0:f15aa1706e16 78 i++;
BAC 0:f15aa1706e16 79 }
BAC 0:f15aa1706e16 80 else{
BAC 0:f15aa1706e16 81 i = 0;
BAC 0:f15aa1706e16 82 }
BAC 0:f15aa1706e16 83
BAC 0:f15aa1706e16 84 printf("\rHi i=%d\r",i);
BAC 0:f15aa1706e16 85 Thread::wait(2000); // wait for 2 seconds
BAC 0:f15aa1706e16 86 }
BAC 0:f15aa1706e16 87 }
BAC 0:f15aa1706e16 88
BAC 0:f15aa1706e16 89 void read_thread(void const *args){
BAC 0:f15aa1706e16 90
BAC 0:f15aa1706e16 91 while(1)
BAC 0:f15aa1706e16 92 {
BAC 0:f15aa1706e16 93 if(j<50){
BAC 0:f15aa1706e16 94 v1 = v1 + Sen1.GetV1();
BAC 0:f15aa1706e16 95 v2 = v2 + Sen1.GetV2();
BAC 0:f15aa1706e16 96 v3 = v3 + Sen1.GetV3();
BAC 0:f15aa1706e16 97 j++;
BAC 0:f15aa1706e16 98 }
BAC 0:f15aa1706e16 99 else{
BAC 0:f15aa1706e16 100 j = 0;
BAC 0:f15aa1706e16 101 v1avg = v1/50;
BAC 0:f15aa1706e16 102 v2avg = v2/50;
BAC 0:f15aa1706e16 103 v3avg = v3/50;
BAC 0:f15aa1706e16 104 //printf("v1avg = %f v2avg = %f v3avg = %f",v1avg,v2avg,v3avg);
BAC 0:f15aa1706e16 105 v1 = 0;
BAC 0:f15aa1706e16 106 v2 = 0;
BAC 0:f15aa1706e16 107 v3 = 0;
BAC 0:f15aa1706e16 108 }
BAC 0:f15aa1706e16 109 Thread::wait(40); // wait for 40 ms
BAC 0:f15aa1706e16 110 }
BAC 0:f15aa1706e16 111 }
BAC 0:f15aa1706e16 112
BAC 0:f15aa1706e16 113 void drive_thread(void const *args){
BAC 0:f15aa1706e16 114 while(1)
BAC 0:f15aa1706e16 115 {
BAC 0:f15aa1706e16 116 Mset1.DriveTable(i);
BAC 0:f15aa1706e16 117 printf("\rHi duty=%d\r",i);
BAC 0:f15aa1706e16 118 printf("\rdu1[%d] = %d du2[%d] = %d du3[%d] = %d\r",i,Mset1.Getdu1(i),i,Mset1.Getdu2(i),i,Mset1.Getdu3(i));
BAC 0:f15aa1706e16 119 Thread::wait(1000); // wait for 1 seconds
BAC 0:f15aa1706e16 120 }
BAC 0:f15aa1706e16 121 }