Robotics Studio
/
NucleoBoard_1
Board1
main.cpp@2:2ef793d0ac01, 2019-04-20 (annotated)
- Committer:
- choyai
- Date:
- Sat Apr 20 09:35:37 2019 +0000
- Revision:
- 2:2ef793d0ac01
- Parent:
- 1:4eeae0f92c4c
buggy version
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pbdt1997 | 0:97879c8efe59 | 1 | //Module8-9 |
pbdt1997 | 0:97879c8efe59 | 2 | //Nucleo Board 1 (Master) |
pbdt1997 | 0:97879c8efe59 | 3 | |
pbdt1997 | 0:97879c8efe59 | 4 | #include "mbed.h" |
pbdt1997 | 0:97879c8efe59 | 5 | #include <Map.hpp> |
pbdt1997 | 0:97879c8efe59 | 6 | |
pbdt1997 | 0:97879c8efe59 | 7 | Serial pc(USBTX, USBRX); |
pbdt1997 | 0:97879c8efe59 | 8 | |
pbdt1997 | 0:97879c8efe59 | 9 | SPI master(PA_7, PA_6, PA_5); //SPI1_MOSI, SPI1_MISO, SPI1_SCLK |
pbdt1997 | 1:4eeae0f92c4c | 10 | SPI mstrEnc2(PB_15, PB_14, PB_13); //SPI2_MOSI, SPI2_MISO, SPI2_SCLK |
pbdt1997 | 1:4eeae0f92c4c | 11 | SPI mstrEnc1(PC_12, PC_11, PC_10); //SPI3_MOSI, SPI3_MISO, SPI3_SCLK |
pbdt1997 | 0:97879c8efe59 | 12 | |
pbdt1997 | 0:97879c8efe59 | 13 | DigitalOut csMstr(PA_15); //SPI1_SSEL Master |
pbdt1997 | 1:4eeae0f92c4c | 14 | DigitalOut csEnc2(PB_12); //SPI2_SSEL Encoder 1 |
pbdt1997 | 1:4eeae0f92c4c | 15 | DigitalOut csEnc1(PA_4); //SPI3_SSEL Encoder 2 |
pbdt1997 | 0:97879c8efe59 | 16 | |
choyai | 2:2ef793d0ac01 | 17 | Ticker encTicker; |
choyai | 2:2ef793d0ac01 | 18 | |
choyai | 2:2ef793d0ac01 | 19 | |
pbdt1997 | 1:4eeae0f92c4c | 20 | DigitalOut dir1(PC_0); |
pbdt1997 | 1:4eeae0f92c4c | 21 | PwmOut pwm1(PC_9); |
pbdt1997 | 1:4eeae0f92c4c | 22 | |
pbdt1997 | 1:4eeae0f92c4c | 23 | DigitalOut dir2(PC_1); |
pbdt1997 | 1:4eeae0f92c4c | 24 | PwmOut pwm2(PC_8); |
pbdt1997 | 0:97879c8efe59 | 25 | |
pbdt1997 | 0:97879c8efe59 | 26 | //Parameters |
pbdt1997 | 0:97879c8efe59 | 27 | |
pbdt1997 | 0:97879c8efe59 | 28 | char array[4]; |
pbdt1997 | 0:97879c8efe59 | 29 | char arraySize = sizeof(array); |
pbdt1997 | 0:97879c8efe59 | 30 | char getData = 0; |
pbdt1997 | 0:97879c8efe59 | 31 | char checkData = 0; |
pbdt1997 | 0:97879c8efe59 | 32 | char encArr[2]= {255,255}; |
pbdt1997 | 0:97879c8efe59 | 33 | char encLength = 2; |
pbdt1997 | 0:97879c8efe59 | 34 | bool start = false; |
choyai | 2:2ef793d0ac01 | 35 | float q1 = 0; |
choyai | 2:2ef793d0ac01 | 36 | float q2 = 0; |
pbdt1997 | 0:97879c8efe59 | 37 | |
pbdt1997 | 1:4eeae0f92c4c | 38 | |
pbdt1997 | 0:97879c8efe59 | 39 | ///////////////////////////// |
pbdt1997 | 0:97879c8efe59 | 40 | |
pbdt1997 | 0:97879c8efe59 | 41 | //Functions |
pbdt1997 | 0:97879c8efe59 | 42 | |
pbdt1997 | 1:4eeae0f92c4c | 43 | //store data from pc |
pbdt1997 | 0:97879c8efe59 | 44 | void RX(int data){ |
pbdt1997 | 0:97879c8efe59 | 45 | array[checkData] = data; |
pbdt1997 | 0:97879c8efe59 | 46 | checkData++; |
pbdt1997 | 0:97879c8efe59 | 47 | if(checkData == arraySize){ |
pbdt1997 | 0:97879c8efe59 | 48 | getData = 1; |
pbdt1997 | 0:97879c8efe59 | 49 | checkData = 0; |
pbdt1997 | 0:97879c8efe59 | 50 | } |
pbdt1997 | 0:97879c8efe59 | 51 | } |
pbdt1997 | 0:97879c8efe59 | 52 | |
pbdt1997 | 1:4eeae0f92c4c | 53 | //receive data from pc |
pbdt1997 | 0:97879c8efe59 | 54 | void RX_INT(){ |
pbdt1997 | 0:97879c8efe59 | 55 | int data = pc.getc(); |
pbdt1997 | 0:97879c8efe59 | 56 | RX(data); |
pbdt1997 | 0:97879c8efe59 | 57 | } |
pbdt1997 | 0:97879c8efe59 | 58 | |
pbdt1997 | 1:4eeae0f92c4c | 59 | //convert 14bit data to angle |
pbdt1997 | 0:97879c8efe59 | 60 | float ConvertAngle(int encData){ |
pbdt1997 | 0:97879c8efe59 | 61 | Map map(0, 16383, 0, 359); |
pbdt1997 | 0:97879c8efe59 | 62 | float angle = map.Calculate(encData); |
pbdt1997 | 0:97879c8efe59 | 63 | return angle; |
pbdt1997 | 0:97879c8efe59 | 64 | } |
pbdt1997 | 0:97879c8efe59 | 65 | |
pbdt1997 | 1:4eeae0f92c4c | 66 | //read encoder of joint 1 |
pbdt1997 | 0:97879c8efe59 | 67 | float readEnc1(){ |
pbdt1997 | 0:97879c8efe59 | 68 | csEnc1 = 0; |
pbdt1997 | 0:97879c8efe59 | 69 | char *arrAdd = &encArr[0]; |
pbdt1997 | 0:97879c8efe59 | 70 | int encData = mstrEnc1.write(arrAdd, 2, arrAdd, 2); |
pbdt1997 | 0:97879c8efe59 | 71 | encData = encArr[0] * 256 + encArr[1]; |
pbdt1997 | 0:97879c8efe59 | 72 | csEnc1 = 1; |
pbdt1997 | 0:97879c8efe59 | 73 | return ConvertAngle(encData/2); |
pbdt1997 | 0:97879c8efe59 | 74 | } |
pbdt1997 | 0:97879c8efe59 | 75 | |
pbdt1997 | 1:4eeae0f92c4c | 76 | //read encoder of joint 2 |
pbdt1997 | 0:97879c8efe59 | 77 | float readEnc2(){ |
pbdt1997 | 0:97879c8efe59 | 78 | csEnc2 = 0; |
pbdt1997 | 0:97879c8efe59 | 79 | char *arrAdd = &encArr[0]; |
pbdt1997 | 0:97879c8efe59 | 80 | int encData = mstrEnc2.write(arrAdd, 2, arrAdd, 2); |
pbdt1997 | 0:97879c8efe59 | 81 | encData = encArr[0] * 256 + encArr[1]; |
pbdt1997 | 0:97879c8efe59 | 82 | csEnc2 = 1; |
pbdt1997 | 0:97879c8efe59 | 83 | return ConvertAngle(encData/2); |
pbdt1997 | 0:97879c8efe59 | 84 | } |
pbdt1997 | 0:97879c8efe59 | 85 | |
choyai | 2:2ef793d0ac01 | 86 | //ISR reads encoders every 20us |
choyai | 2:2ef793d0ac01 | 87 | void readEncoders(){ |
choyai | 2:2ef793d0ac01 | 88 | q1 = readEnc1(); |
choyai | 2:2ef793d0ac01 | 89 | q2 = readEnc2(); |
choyai | 2:2ef793d0ac01 | 90 | } |
choyai | 2:2ef793d0ac01 | 91 | |
pbdt1997 | 1:4eeae0f92c4c | 92 | //drive motor joint1 |
pbdt1997 | 1:4eeae0f92c4c | 93 | void drvMotor1(float duty){ |
pbdt1997 | 1:4eeae0f92c4c | 94 | if(duty > 0.9){ |
pbdt1997 | 1:4eeae0f92c4c | 95 | duty = 0.9; |
pbdt1997 | 1:4eeae0f92c4c | 96 | } |
pbdt1997 | 1:4eeae0f92c4c | 97 | else if(duty < -0.9){ |
pbdt1997 | 1:4eeae0f92c4c | 98 | duty = -0.9; |
pbdt1997 | 1:4eeae0f92c4c | 99 | } |
pbdt1997 | 1:4eeae0f92c4c | 100 | if(duty >= 0){ |
pbdt1997 | 1:4eeae0f92c4c | 101 | dir1 = 1; //CW |
pbdt1997 | 1:4eeae0f92c4c | 102 | |
pbdt1997 | 1:4eeae0f92c4c | 103 | } |
pbdt1997 | 1:4eeae0f92c4c | 104 | else if(duty < 0){ |
pbdt1997 | 1:4eeae0f92c4c | 105 | dir1 = 0; //CCW |
pbdt1997 | 1:4eeae0f92c4c | 106 | duty = -duty; |
pbdt1997 | 1:4eeae0f92c4c | 107 | } |
pbdt1997 | 1:4eeae0f92c4c | 108 | pwm1.period_ms(1.0f); |
pbdt1997 | 1:4eeae0f92c4c | 109 | pwm1.write(duty); |
pbdt1997 | 1:4eeae0f92c4c | 110 | } |
pbdt1997 | 1:4eeae0f92c4c | 111 | |
choyai | 2:2ef793d0ac01 | 112 | //drive motor joint2 |
pbdt1997 | 1:4eeae0f92c4c | 113 | void drvMotor2(float duty){ |
pbdt1997 | 1:4eeae0f92c4c | 114 | if(duty > 0.5){ |
pbdt1997 | 1:4eeae0f92c4c | 115 | duty = 0.5; |
pbdt1997 | 1:4eeae0f92c4c | 116 | } |
pbdt1997 | 1:4eeae0f92c4c | 117 | else if(duty < -0.5){ |
pbdt1997 | 1:4eeae0f92c4c | 118 | duty = -0.5; |
pbdt1997 | 1:4eeae0f92c4c | 119 | } |
pbdt1997 | 1:4eeae0f92c4c | 120 | if(duty >= 0){ |
choyai | 2:2ef793d0ac01 | 121 | dir2 = 0; //CCW |
pbdt1997 | 1:4eeae0f92c4c | 122 | } |
pbdt1997 | 1:4eeae0f92c4c | 123 | else if(duty < 0){ |
choyai | 2:2ef793d0ac01 | 124 | dir2 = 1; //CW |
pbdt1997 | 1:4eeae0f92c4c | 125 | duty = -duty; |
pbdt1997 | 1:4eeae0f92c4c | 126 | } |
pbdt1997 | 1:4eeae0f92c4c | 127 | pwm2.period_ms(1.0f); |
pbdt1997 | 1:4eeae0f92c4c | 128 | pwm2.write(duty); |
pbdt1997 | 1:4eeae0f92c4c | 129 | } |
pbdt1997 | 1:4eeae0f92c4c | 130 | |
pbdt1997 | 1:4eeae0f92c4c | 131 | //PID of joint 1 |
pbdt1997 | 1:4eeae0f92c4c | 132 | void PID_1(float pos){ |
pbdt1997 | 1:4eeae0f92c4c | 133 | pos = -pos; |
pbdt1997 | 1:4eeae0f92c4c | 134 | const float Kp=0.5,Ki=0,Kd=0.1; |
pbdt1997 | 1:4eeae0f92c4c | 135 | float u,e,p,s; |
pbdt1997 | 1:4eeae0f92c4c | 136 | float offset = 110.66; |
pbdt1997 | 1:4eeae0f92c4c | 137 | bool stop1 = false; |
pbdt1997 | 1:4eeae0f92c4c | 138 | |
pbdt1997 | 1:4eeae0f92c4c | 139 | while(stop1==false){ |
choyai | 2:2ef793d0ac01 | 140 | printf("Encoder1: %.2f ", q1); |
choyai | 2:2ef793d0ac01 | 141 | printf("Encoder2: %.2f\n", q2); |
choyai | 2:2ef793d0ac01 | 142 | if(q1 < 110){ |
choyai | 2:2ef793d0ac01 | 143 | q1 = q1 + 360; |
pbdt1997 | 1:4eeae0f92c4c | 144 | } |
choyai | 2:2ef793d0ac01 | 145 | e = (pos - q1) + offset; |
pbdt1997 | 1:4eeae0f92c4c | 146 | s = s + e; |
pbdt1997 | 1:4eeae0f92c4c | 147 | if(abs(e)>0.5){ |
pbdt1997 | 1:4eeae0f92c4c | 148 | u = (Kp*e) + (Ki*s) + (Kd*(e-p)); |
pbdt1997 | 1:4eeae0f92c4c | 149 | } |
pbdt1997 | 1:4eeae0f92c4c | 150 | else{ |
pbdt1997 | 1:4eeae0f92c4c | 151 | u = 0; |
pbdt1997 | 1:4eeae0f92c4c | 152 | drvMotor1(0); |
pbdt1997 | 1:4eeae0f92c4c | 153 | stop1 = true; |
pbdt1997 | 1:4eeae0f92c4c | 154 | } |
pbdt1997 | 1:4eeae0f92c4c | 155 | drvMotor1(u); |
pbdt1997 | 1:4eeae0f92c4c | 156 | p = e; |
pbdt1997 | 1:4eeae0f92c4c | 157 | } |
pbdt1997 | 1:4eeae0f92c4c | 158 | } |
pbdt1997 | 1:4eeae0f92c4c | 159 | |
pbdt1997 | 1:4eeae0f92c4c | 160 | //PID of joint 2 |
pbdt1997 | 1:4eeae0f92c4c | 161 | void PID_2(float pos){ |
pbdt1997 | 1:4eeae0f92c4c | 162 | const float Kp=1,Ki=0,Kd=0.1; |
pbdt1997 | 1:4eeae0f92c4c | 163 | float u,e,p,s; |
pbdt1997 | 1:4eeae0f92c4c | 164 | float offset = 21.52 - 135; |
pbdt1997 | 1:4eeae0f92c4c | 165 | bool stop = false; |
pbdt1997 | 1:4eeae0f92c4c | 166 | pos=-pos; |
pbdt1997 | 1:4eeae0f92c4c | 167 | while(stop==false){ |
choyai | 2:2ef793d0ac01 | 168 | printf("Encoder1: %.2f ", q1); |
choyai | 2:2ef793d0ac01 | 169 | printf("Encoder2: %.2f\n", q2); |
choyai | 2:2ef793d0ac01 | 170 | e = (pos - q2) + offset; |
pbdt1997 | 1:4eeae0f92c4c | 171 | s = s + e; |
pbdt1997 | 1:4eeae0f92c4c | 172 | if(abs(e)>0.5){ |
pbdt1997 | 1:4eeae0f92c4c | 173 | u = (Kp*e) + (Ki*s) + (Kd*(e-p)); |
pbdt1997 | 1:4eeae0f92c4c | 174 | } |
pbdt1997 | 1:4eeae0f92c4c | 175 | else{ |
pbdt1997 | 1:4eeae0f92c4c | 176 | u = 0; |
pbdt1997 | 1:4eeae0f92c4c | 177 | drvMotor2(0); |
pbdt1997 | 1:4eeae0f92c4c | 178 | stop = true; |
pbdt1997 | 1:4eeae0f92c4c | 179 | } |
pbdt1997 | 1:4eeae0f92c4c | 180 | drvMotor2(u); |
pbdt1997 | 1:4eeae0f92c4c | 181 | p = e; |
pbdt1997 | 1:4eeae0f92c4c | 182 | } |
pbdt1997 | 1:4eeae0f92c4c | 183 | } |
pbdt1997 | 1:4eeae0f92c4c | 184 | //Set Home |
pbdt1997 | 1:4eeae0f92c4c | 185 | void setHome(){ |
pbdt1997 | 1:4eeae0f92c4c | 186 | PID_1(0); |
pbdt1997 | 1:4eeae0f92c4c | 187 | PID_2(0); |
pbdt1997 | 1:4eeae0f92c4c | 188 | } |
pbdt1997 | 1:4eeae0f92c4c | 189 | |
pbdt1997 | 0:97879c8efe59 | 190 | //Main |
pbdt1997 | 0:97879c8efe59 | 191 | |
pbdt1997 | 0:97879c8efe59 | 192 | int main() { |
choyai | 2:2ef793d0ac01 | 193 | encTicker.attach_us(&readEncoders, 2.0); |
choyai | 2:2ef793d0ac01 | 194 | |
pbdt1997 | 1:4eeae0f92c4c | 195 | pc.baud(256000); |
pbdt1997 | 0:97879c8efe59 | 196 | RX_INT(); |
pbdt1997 | 0:97879c8efe59 | 197 | pc.attach(&RX_INT, Serial::RxIrq); |
pbdt1997 | 0:97879c8efe59 | 198 | |
pbdt1997 | 0:97879c8efe59 | 199 | csMstr = 1; |
pbdt1997 | 0:97879c8efe59 | 200 | csEnc1 = 1; |
pbdt1997 | 0:97879c8efe59 | 201 | csEnc2 = 1; |
pbdt1997 | 0:97879c8efe59 | 202 | |
pbdt1997 | 0:97879c8efe59 | 203 | master.format(8,3); |
pbdt1997 | 0:97879c8efe59 | 204 | master.frequency(1000000); |
pbdt1997 | 0:97879c8efe59 | 205 | mstrEnc1.format(14, 3); |
pbdt1997 | 0:97879c8efe59 | 206 | mstrEnc1.frequency(500000); |
pbdt1997 | 0:97879c8efe59 | 207 | mstrEnc2.format(14, 3); |
pbdt1997 | 0:97879c8efe59 | 208 | mstrEnc2.frequency(500000); |
pbdt1997 | 0:97879c8efe59 | 209 | |
pbdt1997 | 0:97879c8efe59 | 210 | //Program Loop |
pbdt1997 | 0:97879c8efe59 | 211 | while(true){ |
pbdt1997 | 0:97879c8efe59 | 212 | if(getData == 1){ |
pbdt1997 | 0:97879c8efe59 | 213 | for(char i = 0; i < arraySize; i++){ |
pbdt1997 | 0:97879c8efe59 | 214 | pc.putc(array[i]); |
pbdt1997 | 0:97879c8efe59 | 215 | } |
pbdt1997 | 0:97879c8efe59 | 216 | if(array[2] == 122){ //receive 'z' |
pbdt1997 | 0:97879c8efe59 | 217 | start = true; |
pbdt1997 | 0:97879c8efe59 | 218 | csMstr = 0; |
pbdt1997 | 0:97879c8efe59 | 219 | master.write(array[3]); |
pbdt1997 | 0:97879c8efe59 | 220 | int masterData = master.write(0x00); |
pbdt1997 | 0:97879c8efe59 | 221 | csMstr = 1; |
pbdt1997 | 0:97879c8efe59 | 222 | pc.putc(masterData); |
pbdt1997 | 0:97879c8efe59 | 223 | pc.putc('A'); |
pbdt1997 | 1:4eeae0f92c4c | 224 | } |
pbdt1997 | 1:4eeae0f92c4c | 225 | if(array[2] == 121){ //'y' |
pbdt1997 | 1:4eeae0f92c4c | 226 | setHome(); |
pbdt1997 | 0:97879c8efe59 | 227 | } |
choyai | 2:2ef793d0ac01 | 228 | if(array[2] == 120){ //'x' |
choyai | 2:2ef793d0ac01 | 229 | printf('q1 = %.2f, q2 = %.2) |
pbdt1997 | 0:97879c8efe59 | 230 | |
pbdt1997 | 0:97879c8efe59 | 231 | getData = 0; |
pbdt1997 | 0:97879c8efe59 | 232 | } |
pbdt1997 | 0:97879c8efe59 | 233 | |
pbdt1997 | 0:97879c8efe59 | 234 | } |
pbdt1997 | 0:97879c8efe59 | 235 | |
pbdt1997 | 0:97879c8efe59 | 236 | } |